Method of curvature controlled data smoothing

ABSTRACT

The present invention involves a method of adjusting path data for a motive body includes identifying four consecutive points of the path data. The four consecutive points include a first point, a second point, a third point and a fourth point. It is determined whether the second point is suitable for smoothing. If the second point is suitable for smoothing, then a first arc and a second arc are located, and the second point is moved into an area between the first arc and the second arc. The first arc is defined by the first point, the second point and the fourth point. The second arc is defined by the first point, the third point and the fourth point. A line move associated with the second point is replaced with at least one arc move associated with the second point.

CROSS REFERENCE TO RELATED APPLICATIONS

This application claims the benefit under Title 35, U.S.C. § 119(e) ofU.S. Provisional Patent Application Ser. No. 60/664,398, entitled METHODOF TRAJECTORY MOTION CONTROL, filed on Mar. 23, 2005.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to navigation of a path by a movingobject, and, more particularly, to motion control systems forcomputer-controllable machine tools.

2. Description of the Related Art

Automation has resulted in the development of motion controllers capableof signaling actuator devices to effect motion in linkages along adesired trajectory while performing useful work. Motion controllerspermit increased speed and precision in performing a given task overmanual operation. Robots and automated manufacturing equipment areexamples of a few of the products that utilize motion controltechnology. Programming these devices is often accomplished byspecifying the desired trajectory as a collection of line/arc segments,along with the desired velocity of a tool along each segment. Thevelocity of the tool is often kept constant along each segment or groupof segments of complex trajectories because velocity optimization alongeach point in the trajectory would be very time consuming.

Most humans who program tool trajectory have a fundamental understandingof the trade-off between velocity and accuracy. It is well known that athigher velocities it becomes more difficult for the control system tostay on the desired trajectory. Thus, trajectory programmers must make atrade-off between the velocity and the precision of motion along thedesired trajectory. These decisions are often based on the programmer'sexperience and result in an iterative programming process wherein thetrajectory is executed and then modified to reduce the velocity insections where an undesirable deviation from the desired trajectory isobserved. Thus, programmers control the deviation from the desiredtrajectory, and therefore the quality of the motion, by manipulating thetool velocity along the trajectory.

Motion control systems for manufacturing equipment, often referred to asComputer Numerical Controllers (CNCs), attempt to maximize the velocityof motion control while minimizing the deviation from the desiredtrajectory. CNCs may be used to control manufacturing equipment such aslathes, grinders and mills. CNCs are computing devices adapted for thereal-time control of machine tools. A numerical controller receives aset of coded instructions that form a part program. Part programs aretypically expressed in a standard G&M code language, or a closederivative of this language based on either the International StandardsOrganization (ISO) or the Electronics Industries Association (EIA)RS-274-D, using codes identified by letters such as G, M, or F. Thecodes define a sequence of machining operations to control motion in themanufacture of a part. The numerical controller converts the codes to aseries of electrical signals which control motors attached to a machinetool effecting the motion of the tool along the programmed trajectory.

A motion controller operating a milling machine is one example of CNC.Lathes, grinders and coordinate measuring machines (CMMS) are otherexamples of manufacturing equipment which utilize a CNC for motioncontrol. A three-axis CNC milling machine has a head where a tool ismounted, and a table movable relative to the tool in the X, Y plane.Motors control motion of the table in the X and Y directions and motionof tool in the Z direction, establishing an orthogonal X, Y, Z Cartesiancoordinate system. Positional sensors (encoders or scales, typically)provide feedback indicating the position of the tool with respect to thecoordinate system of the milling machine. The CNC reads in a partprogram specifying a tool path trajectory that the tool is to follow ata specified velocity or feedrate. The controller continuously comparesthe current tool position with the specified tool path. Using thisfeedback, the controller generates signals to control motors in such away that the tool's actual trajectory matches the desired tool path ortrajectory as closely as possible while the tool moves along the toolpath at the desired velocity. The controller may be used in conjunctionwith a computer aided machining (CAM) system.

The deviation of the actual tool trajectory from the desired trajectoryor tool path is referred to as “machining error.” The machining errormay be computed as the distance between the instantaneous tool positionand the desired trajectory as specified by the tool path. CNC toleranceis defined as the amount of the permitted machining error whilemachining. Motion controllers are expected to maintain good or tight CNCtolerance. The machining error depends on many factors including theperformance of the motion controller and the feedrate selected fortraversing the trajectory during machining. In general, higher feedrateswill result in larger machining errors.

Known part programs do not explicitly address CNC tolerance issues. Themachine tool operator, part programmer or machinist must set feedratesto attempt to address these issues. In fact, tolerance cannot beexpressed using known CNC programming languages, such as EIA RS-274-D,nor do existing motion controllers support the notion of constrainingmotion so that a CNC tolerance specification is met. One of theoperator's functions is to select feedrates that would result inacceptable part quality, while simultaneously achieving high metalremoval rates. The selection of appropriate feedrates is based on theoperator's experience, and general rules of thumb may be obtained fromnumerous handbooks and charts (e.g., Machinery's Handbook, 24th edition,Industrial Press Inc., New York 1992). However, the figures from suchdocuments, while perfectly feasible when used under the correctconditions, are frequently inappropriate when applied to specificmachining situations. Published figures fail to account for localmachining conditions such as sudden changes in the tool path, therebyleaving optimization of the feedrate to the operator. It is difficultfor an operator to select feedrate values that achieve the desired partquality while maximizing the machine tool's productivity throughout thepart program.

A tolerance based motion control system, including a method for settingfeedrates based upon tolerance restraints, is disclosed in U.S. Pat. No.6,242,880, which is incorporated herein by reference. While this patentrepresents a significant step forward in the art of motion control,refinements in the use of tolerance commands, E codes, and toleranceregions are needed in order to improve feedrates while still operatingwithin the tolerance limits.

SUMMARY OF THE INVENTION

The present invention applies smoothing to any trajectory of a motivebody. For example, the present invention may be applied to smooth thetrajectory of any motive body, such as a vehicle or projectile,regardless of whether the vehicle or projectile travels by land, sea orair. In a specific implementation of the present invention, the methodmay also be applied to adjust the feedrate of a CNC machine tool basedon the CNC tolerance specified by the programmer and the programmed toolpath, defining the maximum allowable feedrate at each point along theprogrammed trajectory. This information, along with position feedback,is used to modify the feedrate along the actual trajectory of motion tolimit the position deviation from the ideal programmed path so that therequested CNC tolerance can be achieved.

Tolerance Based Control (TBC) Technology introduces the notion of CNCTolerance to the programming and machining environment. It effectivelyallows the CNC programmer to program a part by using the maximum, ornear maximum, permissible feedrates for a given cutting condition, i.e.cutter, speed, depth of cut, material conditions and so forth, andspecify a desired CNC Tolerance.

The Tolerance Control may calculate feedrate limitations based on theCNC tolerance specified by the programmers and the programmed tool path,specifying the maximum, or near maximum, allowable feedrate at eachpoint along the programmed trajectory. This information along withposition feedback may be used to modify the feedrate along the actualtrajectory of motion to limit position deviations from the ideallyprogrammed path so that the requested CNC tolerance may be satisfied.

Unlike the classical Proportional, Integral and Derivative (PID)approach, which includes guessing where the cutter tool will be atvarious time intervals, the TBC approach may use Predictive Controltechniques, which replaces non-TBC point-to-point fixed cycle controlwith trajectory and velocity based motion control. TBC may includelooking at where the cutter tool is in relation to the programmedtrajectory and adjusting accordingly, thereby eliminating the need forfollowing error terms in the control loop. TBC continually processesmachine tool position feedback along with the programmed trajectory andknowledge of a machine's capabilities to instantaneously issue a controlsignal that is responsive to the instantaneous machining conditions.Where a non-TBC PID control loop approximates the programmed trajectoryby pulling the cutter tool from preplanned target to target using thefollowing error, the TBC control pushes the cutter tool along thedesired trajectory, correcting for deviations as they are encountered.TBC control may include controlling or adjusting movements of the toolbased upon the current tracked position of the tool vis-a-vis a desiredtool trajectory, rather than basing future tool movements solely uponwhere the tool should be at a certain point in time according to apre-planned itinerary.

In the “true arc” technology employed by the present invention, toolmotion is controlled along a trajectory that is modeled as an arcuatepath without polygon approximation. That is, the tool may be commandedto follow an arcuate trajectory rather than a trajectory formed by aseries of linear segments that approximate a curved trajectory. By useof arcuate trajectories and associated vectors in real time instead ofsegment-based trajectories, TBC control may eliminate the conversion ofarc trajectories into a straight-line approximation of the arc, allowingfor the direct precision machining of arc segments, the elimination ofthe chord error, the reduction of part program size and the eliminationof the processing required to convert complex trajectories into theirpolynomial approximations.

The Data Smoothing technology in TBC control may include examining atarget trajectory, for example a programmed NC tool trajectoryidentifying NC line segments which can be smoothed, and converting theseNC line data into arcs while maintaining a given tolerance. DataSmoothing may include converting point-to-point linear moves into asequence of mutually tangential arcs within allowed tolerances and withrelatively small curvature changes. The extent of smoothing may bedependent on the specified tolerance. This technology has the benefit ofachieving smoother trajectory, smoother velocity, acceleration, betterfeedrate control and improved surface finish. The smoothing technologyis particularly important with legacy part programs expressing complexpart geometry using polygon approximations.

The lookahead technology in TBC replaces non-TBC fixed buffer lookaheadalgorithms, reducing the possibility of data starvation, overshooting,tolerance breaching, dwell marks and poor surface finish on partproducts. Lookahead may ensure enough distance to accelerate ordecelerate from one speed to another. When the length of a motionsegment is relatively short and not enough for a large speed change, thelookahead algorithm may calculate and limit the speed of that motionsegment according to some motion profile, such as an S-curve speedprofile. Because lookahead is usually performed in real time andincludes recursive modification of previously processed motion commands,the high efficiency of the algorithm may result in reduction of the CPUload and an improvement in machine performance. The recursivemodification of motions may involve only logic and addition. Moreover,the algorithm may have the advantage of requiring fewer iterations ofmodifying previous commands. The algorithm may have a further advantageof improved run times which increases the system speed or throughput.

One option in a non-TBC Lookahead algorithm is to calculate a speedlimit upon each instance of the motion distance being found insufficientto achieve a desired speed change. Such calculation of the speed limitmay involve a large amount of computation. Repeatedly performing thecalculation may be undesirable because of the processing time andresources required. The additive lookahead algorithm of the presentinvention may use a stop distance concept as a metric of speedlimitation, which may be very computationally efficient.

The Soft-motion technology is the basis of the TBC control, which iseasily updated and eliminates the need for costly PID based motioncontrol cards.

The TBC Prototype Software System may include Win32 components and RealTime Software System (RTSS) components. The term “Win32” refers to anapplication program interface (API) for running 32-bit Windowsapplications under Windows NT and Windows XP, as is commonly known,although other interfaces may be used with other underlying operatingsystems. Win32 Components may be interface modules for processing userrequests, providing status information back to the users, updatinggraphics display, and converting a subset of G&M code into the requiredforms. Win32 components may include man-machine interface components andan interpreter of a subset of G&M code. The man-machine interfacecomponents may include display software and control panel processing.

The machine I/O software components may include drivers for the servotransducer board. The Machine I/O Servo Driver may be responsible fortaking the control signals generated by the TBC control and processingthem into electrical signals effecting motion. This driver may also takefeedback signals from the machine tool and convert them into informationused by the TBC software.

RTSS Components may be coded within the RTSS, ensuring deterministicresponse. The RTSS components may include TBC components, machine I/Osoftware components, and component integration and communicationcomponents. The TBC components may include tolerance understandingcomponents, data compression components, data smoothing components,lookahead components, and motion kernel components. The TBC componentsmay be functionally divided into two groups, i.e., a TBC datapreprocessing group and a TBC motion control group including TBC motionkernel. The TBC data preprocessing group may include toleranceunderstanding components, data compression components, data smoothingcomponents, and lookahead components. The TBC data preprocessing groupmay be implemented in a queue module.

The smoothing function includes three phases: prepare for smoothing,adjust for smoothing and double arc smoothing. After the three phases ofsmoothing operations, point-to-point moves are converted into a sequenceof mutually tangential arcs within allowed tolerance and with relativelysmall curvature changes.

The invention comprises, in one form thereof, a method for adjustingpath data for a motive body, including identifying three consecutivepoints of the path data. A first distance between a circular arc definedby the three points and a middle portion of a first line extendingbetween a first of the points and a second of the points is ascertained.A second distance between the circular arc and a middle portion of asecond line extending between the second point and a third of the pointsis ascertained. A turn angle between the first line and the second lineis determined. Smoothing is performed on the second point if the firstdistance and/or the second distance is less than a threshold linedistance, and the turn angle is greater than a threshold angle.

The invention comprises, in another form thereof, a method of adjustingpath data of a motive body, including identifying four consecutivepoints of the path data. The four consecutive points include a firstpoint, a second point, a third point and a fourth point. It isdetermined whether the second point is suitable for smoothing. If thesecond point is suitable for smoothing, then a first arc and a secondarc are located, and the second point is moved into an area between thefirst arc and the second arc. The first arc is defined by the firstpoint, the second point and the fourth point. The second arc is definedby the first point, the third point and the fourth point. A line moveassociated with the second point is replaced with at least one arc moveassociated with the second point.

The invention comprises, in yet another form thereof, a method foradjusting path data for a motive body, including identifying fourconsecutive points of the path data. The four consecutive points includea first point, a second point, a third point and a fourth point. A firstarc defined by the first point, the second point and the fourth point islocated. A second arc defined by the first point, the third point andthe fourth point is located. The second point is moved into an areabetween the first arc and the second arc.

The invention comprises, in a further form thereof, a method foradjusting path data for a motive body, including identifying a line moveextending from a first point of the path data to a second point of thepath data. The line move is replaced with a first arc move and a secondarc move. The first arc move begins at the first point and terminates ata junction point. The second arc move begins at the junction point andterminates at the second point.

The invention comprises, in still another form thereof, a method ofprocessing path data for a motive body such as a machine tool, includingidentifying a plurality of points along a target tool path, anddetermining a maximum allowable stopping distance of the tool at a lastof the points. A last maximum allowable speed of the tool at the lastpoint such that the tool may come to a stop within the maximum allowablestopping distance is ascertained. An other maximum allowable speed ofthe tool at an other one of the points such that the tool may possiblyslow down to the last maximum allowable speed upon arriving at the lastpoint is established.

The invention comprises, in another form thereof, a method of processingpath data for a machine tool, including identifying a plurality ofpoints along a target tool path, and establishing a maximum allowablelevel of deviation of an actual tool path from the target tool path. Adegree of curvature of the target tool path at a plurality of locationsalong the target tool path is ascertained. A maximum allowable tolerancespeed of the tool at each of the points along the target tool path isdetermined, wherein the maximum allowable tolerance speed is dependentupon the maximum allowable level of deviation and the degree ofcurvature. A maximum allowable stopping distance of the tool at a lastof the points is determined. A last maximum allowable stopping speed ofthe tool at the last point such that the tool may come to a stop withinthe maximum allowable stopping distance is ascertained. An other maximumallowable stopping speed of the tool at each of the points other thanthe last point such that the tool may possibly slow down to the lastmaximum allowable speed upon arriving at the last point is established.A lesser of the maximum allowable tolerance speed and the maximumallowable stopping speed at each of the points is identified.

The invention comprises, in yet another form thereof, a method ofprocessing path data for a machine tool, including identifying aplurality of points along a target tool path. Each selected two of thepoints being separated by a respective segment length. A maximumallowable stopping distance of the tool at a last of the points isdetermined. A maximum allowable speed of the tool at an other one of thepoints such that a required stopping distance of the tool at the otherpoint is equal to or less than a sum of the maximum allowable stoppingdistance at the last point and the segment length between the otherpoint and the last point is ascertained.

The invention comprises, in a further form thereof, a method ofoperating a machine tool, including identifying a target tool path. Thetool is commanded to follow the target tool path from an initialposition. A first actual position of the tool is sensed after thecommanding step. A first target tool subpath from the first actualposition is calculated to correct for predictive errors in movement ofthe tool.

The invention comprises, in still another form thereof, a method ofoperating a machine tool, including empirically determining predictiveerrors in movement of the tool. Actual movements of the tool aremonitored. The actual movements of the tool are controlled dependentupon the empirically determined predictive errors and random errors inthe actual movements of the tool.

The invention comprises, in a still further form thereof, a method ofoperating a machine tool having a plurality of sequential movements,including sensing an actual position of the tool after a first of themovements of the tool. A target position of the tool is calculated basedon the sensing step. The target position is calculated to compensate forpredictive errors and random errors in the movements of the tool. Thesensing and calculating steps are repeated for other ones of themovements of the tool.

The invention comprises, in yet another form thereof, a method ofoperating a machine tool, including sensing an actual position of thetool. A position error between the actual position of the tool and adesired position of the tool is calculated. The position error iscompared to a specified error tolerance. A speed of the tool is adjusteddependent upon the comparing step.

The invention comprises, in another form thereof, a method of operatinga machine tool, including sensing an actual position of the tool alongeach of a plurality of axes. A multi-dimensional position error betweenthe actual position of the tool in each of the axes and amulti-dimensional reference trajectory is calculated. Amulti-dimensional position error signal is produced dependent upon thecalculating step. A speed and/or a direction of the tool are controlleddependent upon the multi-dimensional position error signal.

An advantage of the present invention is that it provides improvedtrajectories or tool paths for the machine tool. More particularly, thetool paths are smoother, thereby permitting higher feedrates andproviding smoother surfaces on the work piece.

BRIEF DESCRIPTION OF THE DRAWINGS

The above mentioned and other features and objects of this invention,and the manner of attaining them, will become more apparent and theinvention itself will be better understood by reference to the followingdescription of an embodiment of the invention taken in conjunction withthe accompanying drawings, wherein:

FIG. 1 is a flow chart of one embodiment of a tolerance based controlmethod of the present invention.

FIG. 2 is a block diagram of one embodiment of a tolerance based controlsystem of the present invention.

FIG. 3 a is a diagram illustrating the mapping of different tolerancevalues to different regions of machining space.

FIG. 3 b is a diagram illustrating a first case of another embodiment ofthe mapping of different tolerance values to different regions ofmachining space.

FIG. 3 c is a diagram illustrating a second case of another embodimentof the mapping of different tolerance values to different regions ofmachining space.

FIG. 3 d is a diagram illustrating yet another embodiment of the mappingof different tolerance values to different regions of machining space inthe case of an arc.

FIG. 4 a is a plot of program data points, illustrating a method of datacompression according to one embodiment of the present invention.

FIG. 4 b is a plot of program data points, illustrating a method of datacompression according to another embodiment of the present invention.

FIG. 5 a is a plot of program data points, illustrating a method ofselecting data points for data smoothing according to one embodiment ofthe present invention.

FIG. 5 b is a plot of program data points, illustrating a method ofselecting data points for data smoothing according to another embodimentof the present invention.

FIG. 6 a is a plot of program data points, illustrating a method ofadjusting line moves for smoothing according to one embodiment of thepresent invention.

FIG. 6 b is a perspective view of a plot of program data points,illustrating a method of adjusting line moves for smoothing according toanother embodiment of the present invention.

FIG. 7 is an enlarged plot of the middle two points of FIG. 6 a.

FIG. 8 is a plot of the program data points of FIG. 6 a along with onesubsequent program data point, illustrating a next step of the method ofFIG. 6 a for adjusting line moves for smoothing.

FIG. 9 is a plot of program data points, illustrating a method ofsmoothing data using a double arc algorithm according to one embodimentof the present invention.

FIG. 10 is a plot of the tool velocity versus time, also referred to asthe S-Curve.

FIG. 11 is a plot of a segment length extending between two consecutivepoints along a tool path.

FIG. 12 is a plot illustrating the deviations of an actual tool pathfrom a desired tool path.

FIG. 13 is a plot of control loop cycle time for the servo signals inone embodiment of the present invention.

FIG. 14 is a plot of program data points, illustrating a method ofpredictive error correction according to one embodiment of the presentinvention.

FIG. 15 is a plot of program data points, illustrating a method ofrandom error correction according to one embodiment of the presentinvention.

FIG. 16 is a flow chart illustrating a method of the present inventionfor executing a motion kernel.

FIG. 17 is a block diagram of one embodiment of a machine of the presentinvention for machining a work piece.

FIG. 18 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIG. 5.

FIG. 19 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIGS. 6 and 7.

FIG. 20 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIG. 9.

FIG. 21 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIGS. 5 through 9.

FIG. 22 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIG. 11.

FIG. 23 is a flow chart of another embodiment of the method of thepresent invention illustrated in FIG. 11.

FIG. 24 is a flow chart of yet another embodiment of the method of thepresent invention illustrated in FIG. 11.

FIG. 25 is a flow chart of one embodiment of the method of the presentinvention illustrated in FIG. 12.

FIG. 26 is a flow chart of another embodiment of the method of thepresent invention illustrated in FIG. 12.

FIG. 27 is a flow chart of yet another embodiment of the method of thepresent invention illustrated in FIG. 12.

FIG. 28 a is a graphical representation of Level 0 data smoothing.

FIG. 28 b is a graphical representation of Level 1 data smoothing.

FIG. 28 c is a graphical representation of Level 2 data smoothing.

FIG. 28 d is a graphical representation of Level 3 data smoothing.

FIG. 28 e is a graphical representation of Level 4 data smoothing.

FIG. 29 is a plot illustrating one step of one embodiment of a doublearc smoothing algorithm of the present invention.

FIG. 30 is a plot illustrating another step of one embodiment of adouble arc smoothing algorithm of the present invention.

FIG. 31 is a plot illustrating yet another step of one embodiment of adouble arc smoothing algorithm of the present invention.

FIG. 32 a is another plot of the tool velocity versus time, alsoreferred to as an S-Curve.

FIG. 32 b is a diagram indicating how distances are related to the plotof FIG. 32 a.

FIG. 33 is a diagram illustrating the concept of stop distance.

FIG. 34 is yet another plot of the tool velocity versus time for bothadditive and non-additive lookahead algorithms.

FIG. 35 a is a flow chart of one embodiment of an additive lookaheadalgorithm of the present invention.

FIG. 35 b is a flow chart of one embodiment of a non-additive lookaheadalgorithm.

FIG. 36 is a table comparing the kinematic characteristics of anon-additive lookahead algorithm and a TBC additive lookahead algorithm.

FIG. 37 is a block diagram of one embodiment of a non-TBC motion controlarrangement of the present invention.

FIG. 38 a is a plot of a reference trajectory y=x.

FIG. 38 b is a plot of the reference trajectory of FIG. 38 a decomposedinto a time-based y-axis trajectory y=kt.

FIG. 38 c is a plot of the reference trajectory of FIG. 38 a decomposedinto a time-based x-axis trajectory x=kt.

FIG. 38 d is a plot of actual trajectories resulting from the referencetrajectories of FIGS. 38 a-c.

FIG. 39 is a block diagram of one embodiment of a TBC motion controlarrangement of the present invention.

FIG. 40 is another block diagram of the TBC motion control arrangementof FIG. 39.

FIG. 41 is a plot illustrating the calculation of the stopper planenormal direction for line motion.

FIG. 42 is a plot illustrating the calculation of the stopper planenormal direction for arc motion.

FIG. 43 is a plot illustrating line motion geometry analysis.

FIG. 44 a is a first plot illustrating arc motion geometry analysis.

FIG. 44 b is a second plot illustrating arc motion geometry analysis.

FIG. 45 is a flow chart illustrating operation of one embodiment of aTBC S-Curve Controller.

FIG. 46 is a block diagram illustrating one embodiment of a TBC feedforward control arrangement of the present invention.

FIG. 47 is a block diagram illustrating one embodiment of an arrangementof the present invention for variable-gain feedback control of normalerror.

Corresponding reference characters indicate corresponding partsthroughout the several views. Although the drawings representembodiments of the present invention, the drawings are not necessarilyto scale and certain features may be exaggerated in order to betterillustrate and explain the present invention. The exemplification setout herein illustrates an embodiment of the invention, in one form, andsuch exemplifications are not to be construed as limiting the scope ofthe invention in any manner.

DESCRIPTION OF THE PRESENT INVENTION

Referring now to the drawings and particularly to FIG. 1, there is shownone embodiment of a tolerance based control method 100 of the presentinvention, including the performance of the major functions of atolerance based controller of the present invention as it machines thepart defined by the part program. While the controller of this disclosedembodiment relates to a particular case of the present invention,dealing with CNC machine controls, the invention may be generallyapplied to motive bodies having various motion control situations as oneof skill in the art would recognize, such as robotic control, vehicularcontrol, projectile control, and the like.

In a first step 102, the controller receives part data from the partprogram. The part program may include data specifying the dimensions,shape, and other physical characteristics of the part or “work piece” tobe machined. In general, a trajectory preprocessing step 104 includescalculating a desired tool path and feedrates that the machine toolshould follow in order to produce the part within a relatively shortperiod of time and within dimensional tolerance constraints. As the partis machined, position feedback specifying the actual position of themachine tool is used in a predictive and random error compensation step106 to modify servo commands to redirect the machine tool to compensatefor, i.e., correct, both predictive or repeatable errors and random ornon-repeatable errors in the actual machine tool path as compared to thecommanded tool path. In a final step 108, the modified servo command istransmitted to the servo amplifier for use in actuating the machinetool.

The method of FIG. 1 is now described in more detail with reference tothe flow diagram of FIG. 2. In general, steps 102 and 104 correspond tothe Queue Manager Task of FIG. 2, and steps 106 and 108 correspond tothe Run System Task of FIG. 2. The part program may provide thelocations of discrete points on the surface of the part, and theNumerical Control may interpolate between these discrete points tothereby define a desired trajectory or tool path formed of contiguouslines and arcs. As used herein, the term “arc” may indicate a segment ofa circle. That is, the arc may have a constant radius. The quality ofthe data in the user part program may be directly linked to the qualityof the cutting results on a machine tool. Data of poor quality oftenresults in a part surface that is not smooth.

The Queue Manager Task preprocesses the part program motion data withfour consecutive operations that transform it into high quality motiondata before the Run System Task executes it. The four operations areTolerance Understanding within Tolerance Queue 202, Data Compressionwithin Compression Queue 204, Data Smoothing within Smoothing Queue 206,and Additive Lookahead within Lookahead Queue 208. The queue manager mayperform the corresponding data processing on each queue and move thedata flow through the consecutive queues in a pipeline manner. The dataprocessing includes the tolerance understanding operation on thetolerance queue, the data compressing operation on the compressionqueue, the smoothing operation on the smoothing queue, and the lookaheadoperation on the lookahead queue.

The queue module may preprocess NC motion commands such as positioning(G00), line (G01), arc (G02, G03), and an NC tolerance command E usedfor the tolerance based control. Use of the TBC technology is consistentwith existing or legacy CNC part programs. This technology allows theoperator to specify NC tolerance commands along with the existing NCcommands for execution. By inserting a new NC Tolerance Command withinan existing G&M code part program, the CNC programmer can specifydifferent CNC Tolerance constraints.

The NC tolerance commands specify a region and a tolerance value to beapplied to that region along with a command identification number. An NCtolerance command is defined as an E code:

E tol Xx1 Yy1 Zz1 Xx2 Yy2 Zz2 Iid

An E code requests that a three-dimensional rectangular region definedby the coordinates of the diagonal corners (x1, y1, z1) and (x2, y2,z2), x1<x2, y1<y2, z1<z2, hold a non-zero NC tolerance value of tol.This specific tolerance command may be identified by an integer valueid.

It is possible for multiple E codes to define overlapping toleranceregions. One point may belong to a number of active NC toleranceregions. In this case, the NC tolerance value at the point is defined asthe tolerance value specified in the latest E code whose region coversthis point.

In a tolerance mapping operation within tolerance queue 202, tolerancevalues are assigned to each line or arc within the tool path. Thetolerance mapping operation processes variable tolerance regions thatare defined in the part program and assigns the required tolerances toeach motion move. Tolerance regions having different values may overlapone another, as shown in FIG. 3 a. Three areas 302, 304, 306 haverespective tolerances specified as 0.001 inch, 0.0005 inch, and 0.0003inch. As a move such as from P₀ to P₁ crosses multiple toleranceregions, the move is split into multiple moves, i.e., from P₀ to P₁′,from P₁′ to P₁″, and from P₁″ to P₁, with each move having a differenttolerance value. The tolerance command E codes in a part program mayspecify regions and tolerance values to be applied to those regions inthe part. The motion commands in the part program may fall intodifferent tolerance regions. Tolerance understanding may identify thecorrect tolerance value required for each motion command according tothe programmed E codes.

In processing a tolerance command, if the command is a new tolerancespecification, a space may be created for a new item in a tolerancetable. In the tolerance table, tolerance values are mapped to respectivetolerance regions. Then, the new item may be filled in with thetolerance specification extracted from the command. Finally, the newitem may be inserted at the beginning of the tolerance table. Afterbeing processed, a tolerance command may be discarded, because theinformation from the command has been stored in the tolerance table.

In processing a line command or an arc command, the required tolerancevalue for the command may be calculated and attached to the command. TheE codes may specify different tolerance regions, and each line or arc ofthe NC program may belong to one or more of these tolerance regions. Thealgorithm for determining the required tolerance for each line or arcmay include checking the tolerance of some sample points on the line orarc. If a line or arc lies in one tolerance region, the sample pointswill have the same tolerance, which will be the required tolerance forthis line or arc. On the other hand, if a line or arc lies across morethan one tolerance region, the sample points will have differenttolerances. In this case, the line will be divided into segments basedon the sample points, with each line segment having a unique requiredtolerance. By the same token, an arc may be segmented by differenttolerances, however, the arc will not be divided. The tightest tolerancevalue will be chosen as the required tolerance for this arc instead.

A routine may be used to determine the required tolerance for a givenpoint. It performs the following operations: 1) take the position of thegiven point as a key value; 2) starting from the beginning, search thetolerance table for the E code whose tolerance region covers the keyvalue (i.e., the given point is located within the tolerance region);and 3) assign the tolerance value of the E code first found as therequired tolerance for the given point.

If a point belongs to several tolerance regions specified by differenttolerance commands, the latest tolerance command is in control. A newtolerance command is always inserted at the beginning of the tolerancetable. Therefore, in the above operation, the first found E code is thelatest E code which specifies the required tolerance for the givenpoint.

Another routine may be used to divide a line into segments according tomultiple tolerance specifications. Yet another routine may be used todetermine the tightest tolerance among the multiple tolerancespecifications on an arc.

Another embodiment of tolerance regions having different values andoverlapping one another is illustrated in FIGS. 3 b-c, FIG. 3 billustrating a first case, and FIG. 3 c illustrating a second case. Inthis embodiment, for purposes of computational efficiency, a line movemay be divided into at most two lines. Only one more point, point p′, isgenerated between points p0 and p1.

In the case of an arc (FIG. 3 d), three equidistant points t2, t3, t4,are added between an arc starting point t1 and an end point t5. Thelowest of the tolerances of the five points defines the tolerance of thearc, as indicated by the equationA1.tol=min {t1, t2, t3, t4, t5}.

After the tolerance understanding operation, the elements to be takenfrom tolerance queue 202 and sent to compression queue 204 are:positioning commands, line commands with required tolerance attached,arc commands with required tolerance attached, and the program endcommand.

Data Compression

In a data compression operation within compression queue 204, theprogram data is compressed. In many part programs, there are short linemoves that result in a poor surface finish on the part. It is possibleto combine some of these short lines into longer lines, which mayimprove the part quality. The data compression operation may determinewhether short lines can be combined into longer lines, and the datacompression operation may perform such data compression if it ispossible to do so. In a data compression operation illustrated in FIG. 4a, points P_(1 and P) ₂ are eliminated and a new line is formed frompoint P₀ to point P₃. Tolerance boundaries or constraints on the toolpath are indicated by dashed lines 402, 404. Tolerance boundaries 402,404 may be smaller by an order of magnitude than user-defined numericalcontrol tolerances by E code.

A line command is a point-to-point move. Combining short line moves intoa longer one may include determining a starting point and an end pointamong the target point sequence for a new line move and removing all theintermediate points. The new, longer line move can replace the old shortline moves only if some conditions are met, including the tolerancecondition, i.e., the distance from each of the removed points to the newline is within a given tolerance.

To do data compression, all line commands may be sequentially examinedin compression queue 204. The compression operation may processpoint-to-point moves in the following steps: 1) examine a sequence ofpoint-to-point moves with the consecutive target points p₀, p₁, p₂, p₃,. . . , p_(n−1), p_(n); and 2) remove the points between p₀ and p_(n) inthe sequence, i.e., p₁, p₂, p₃, . . . , p_(n−1), if the followingconditions are all met:

-   -   For all i (1<=i<n), the distance from p_(i) to the straight line        connecting p₀ and p_(n) is less than a given tolerance    -   For all i (1<=i<n), the perpendicular projection point of p_(i)        on the line connecting p₀ and p_(n) is located between p₀ and        p_(n).    -   For all i (0<=i<=n), the line command with p_(i) as the end        position have the same tolerance value    -   The distance from p₀ to p_(n) is bounded by a predetermined        length

Thus, the line move from p₀ to p_(n) replaces n shorter line moves.After the compression operation, the elements in compression queue 204may be more compact since some consecutive short lines have merged. Allthe elements in compression queue 204, including positioning commands,compressed line commands, arc commands, and the program end command, aresent to smoothing queue 206.

In another data compression embodiment, four tests or criteria are usedto determine whether a next point should be added to the compressionqueue, or whether compression should be performed on only the pointsthat are presently in the compression queue. In the illustration of FIG.4 b, assume that points p0, p1, p2 and p3 have already been found tomeet the compression criteria, and that it is being determined whetherpoint p4 should be added to the compression queue, or whethercompression should be performed on only points p0, p1, p2 and p3. Thefour compression criteria may be as follows:

-   Test 1: |p4-p0|<0.05 inch.-   Test 2: For p3, the angle θ is between −90 degrees and 90 degrees.-   Test 3: p0.tol=p1.tol=p2.tol=p3.tol=p4.tol. That is, an equal    tolerance has been assigned to each point from the tolerance table.-   Test 4: The perpendicular distances from p1, p2 and p3 to the line    p0p4 are less than the specified tolerance.

If the above four tests are satisfied, then p4 is added to thecompression queue. If the above four tests are not satisfied, then p3 isadded to the smoothing queue and p1 and p2 are discarded in compression.Essentially, three segments (p0p1, p1p2 and p2p3) are combined into onesegment (p0p3).

Data Smoothing

Data smoothing operations convert NC line data into arcs whilemaintaining a given tolerance. The data smoothing may advantageouslyresult in smoother velocity and acceleration, better control offeedrates, improved surface finish, and full utilization of the TBC truearc technology.

The following different smoothing levels may be provided: Level 0,including no smoothing and producing a cut polygon; Level 1, in whicharcs replace lines, and arcs may not be connected to each other in mostcases; Level 2, in which arcs replace lines, arcs are connected to eachother, and arcs are not tangential to each other in most cases; Level 3,in which arcs replace lines, arcs are connected to each other, and arcsare tangential to each other; and Level 4 (a smoothing level that may beprovided by the present invention), in which arcs replace lines, arcsare connected to each other, arcs are tangential to each other, and arccurvatures change gradually. The preparatory smoothing operation, thecorrective smoothing operation, and the double arc smoothing may beperformed on the smoothing queue simultaneously and in a pipelinemanner.

FIGS. 28 a-e illustrate level 0, level 1, level 2, level 3, and level 4smoothing, respectively. In level 0-3 smoothing, the output ofsmoothing, the final trajectory, always passes through the data points.In FIG. 28 d, c1 and r1 are the center and radius of the arc defined bythe points p0, p1 and p2; and c2 and r2 are the center and radius of thearc defined by the points p2, p3 and p4. The curvature of an arc isdefined as the inverse of the radius (1/r). Thus, in the level 3 exampleof FIG. 28 d, the relatively large radius change from r1 to r2 resultsin a correspondingly large curvature change between the two arcs.

Level 4 is different from the other smoothing levels in the sense thatthe data points are adjusted (within the specified tolerance) to achievegradual curvature changes. This adjustment of the data points isacceptable in applications such as the multi-axis contouring (or metalcutting using CNC machines) because the adjustments are very small(typically less than 0.0005 inch) and are within a specified tolerance.

The points may be adjusted, both in direction and magnitude, to achievegradual curvature changes such that the trajectory approaches beingelliptical. Gradual change of curvature along the trajectory mayfacilitate motion control. Gradual change of curvature may becharacterized by the curvature changing, or at least possibly changing,after each point along the trajectory. As shown in FIG. 28 e, the radiusof curvature becomes incrementally smaller along the trajectory from p0to p4. That is, the radii become smaller with each point along thetrajectory. In FIG. 28 d, in contrast, the radius of curvature drops inone large step as the trajectory passes through p2. However, it may alsobe possible within level 4 smoothing for the curvature to be the same oneither side of a point. As indicated by the overlapping points in FIG.28 e, the points may be adjusted or moved such that the trajectoryapproaches an elliptical path. However, the points may be adjusted onlywithin a specified tolerance.

Motion of a tool along an arc requires a force, referred to as“centripetal force”, directed toward the center of the arc. In the caseof CNC machines, this force may be provided by the motors. Sudden andrelatively large changes in the centripetal forces (as the motion movesfrom one arc to the next) may result in larger overshoots, which may bedetrimental to the resulting surface finish.

FIG. 28 e illustrates minor adjustments to the positions of points p0 top4 as compared to their earlier positions as shown in FIG. 28 d. Theminor adjustments may result in the points defining a trajectory thatapproaches being elliptical. An elliptical trajectory may be ideal oroptimal because, along an ellipse, the curvature changes continuously.However, how large an adjustment, or much of an adjustment, that isallowed within the scope of the invention may depend upon the tolerance.Thus, the points may not actually fall on an ellipse after theadjustments.

In one embodiment of the present invention, instead of optimizing thecurvature changes, a sub-optimal solution is employed such that thetrajectory includes only circular arcs as opposed to being elliptical.Defining the trajectory as a series of circular arcs may have theadvantage of reducing the computational/mathematical complexity ofcalculating the point adjustments. Thus, even if it is possible toachieve an elliptical trajectory by adjusting the points withintolerances, the points are instead adjusted such that the trajectory isdefined by a series of circular arcs in one embodiment of the presentinvention.

In one embodiment, the motion control is implemented in a 200 usec loop,i.e., the motion control is updated every 200 usec. Thus, there is aneed for computations to be performed efficiently such that thecomputations may be performed every 200 micro seconds. Due tolimitations in the speed of a processor which may perform several otheroperations in addition to motion control, using a trajectory defined bya series of circular arcs (gradual changes in curvature) rather than anelliptical trajectory (continuous change of curvature) may be helpful inupdating the motion control with a particular frequency, such as every200 usec.

Data smoothing may include three phases or operations, namely preparingfor smoothing, adjusting for smoothing, and smoothing using a double arcalgorithm. In a first operation of a smoothing method of the presentinvention, line data is identified for smoothing. It is not alwaysfeasible to convert NC line data into arcs while maintaining a giventolerance, as with, for example, connected lines with sharp angles. Itis sometimes not desirable to smooth NC line data, for example, longline moves. Certain conditions may need to be satisfied before raw linedata can be smoothed. The first operation examines the original linedata identifying the line segments, which meet the conditions forsmoothing.

Prepare for Smoothing

During the first operation, all line move target points are sequentiallyexamined in the smoothing queue. The line segments, which can besmoothed, are determined and the target points of all these lines may betagged as “smoothing” or “no smoothing”.

Let p₁, p₂, and p₃ be three consecutive target points in the smoothingqueue. The point p₂ may be tagged as “smoothing” if the followingconditions are all met:

-   The motion from p₁ to p₂ is a line move-   The motion from p₂ to p₃ is a line move-   The length from p₁ to p₂ is bounded by a constant-   The length from p₂ to p₃ is bounded by a constant-   The perpendicular distance from p₁ to line p₂p₃ is bounded by a    constant-   The perpendicular distance from p₃ to line p₁p₂ is bounded by a    constant-   The angle between line p₁p₂ and line p₂p₃ is bounded by a constant

Otherwise, the point p₂ may be tagged as “no smoothing”. The lineelements in smoothing queue 206 processed by the first operation may beall tagged. In one embodiment, the length from p₁ to p₂ and the lengthfrom p₂ to p₃ are both bounded by a constant of approximately between 1cm and 2 cm; the perpendicular distance from p₁ to line p₂p₃ is boundedby a constant of approximately between 20% and 40% of the length from p₂to p₃; the perpendicular distance from p₃ to line p₁p₂ is bounded by aconstant of approximately between 20% and 40% of the length from p₁ top₂; and the angle between line p₁p₂ and p₂p₃ is bounded by a constantapproximately between 135° and 165°.

In a data smoothing operation within smoothing queue 206, line data isconverted into arcs that are connected and tangent to one another andthat have gradually changing curvature or radius. This provides smoothermotion velocity and acceleration that results in a better part surfacefinish. The process of data smoothing includes a preparing for smoothingstep 210 (FIG. 2), an adjusting for smoothing step 212, and a double arcsmoothing step 214. Smoothing preparation step 210 includes identifyingdata points for smoothing. As illustrated in FIG. 5 a, point p₁ may besuitable for smoothing, and thus may be marked for smoothing, if thelength of the moves p₀ to p₁ and p₁ to p₂, and an angle 502 betweenthese moves, are below specified limits. A maximum distance that pointP₀ can be disposed from point P₁ while keeping the length of the move P₀to P₁ below specified limits is indicated by dashed line 504. Similarly,a maximum distance that point P₂ can be disposed from point P₁ whilekeeping the length of the move P₁ to P₂ below specified limits isindicated by dashed line 506. Although the distance from P₁ indicated bydashed line 504 is smaller than the distance from P₁ indicated by dashedline 506, these distances may be equal. In one embodiment, point P₁ maybe suitable for smoothing if the length of the moves P₀ to P₁ and P₁ toP₂, i.e., the first line distance between points P₀ and P₁ and thesecond line distance between points P₁ to P₂, are both below a limit orthreshold line distance of approximately between one centimeter and twocentimeters, and turn angle 502 is above a limit or threshold angleapproximately between 135° and 165°.

In another embodiment, a further requirement for marking point P₁ forsmoothing is that the perpendicular distance from point P₀ to line P₁P₂,and the perpendicular distance from point P₂ to line P₀P₁, are belowspecified limits or threshold perpendicular distances. In oneembodiment, point P₁ may be marked for smoothing if the perpendiculardistance do from point P₀ to line P₁P₂ is less than approximatelybetween 20% and 40% of the length of line P₁P₂, and if the perpendiculardistance d₂ from point P₂ to line P₀P₁ is less than approximatelybetween 20% and 40% of the length of line P₀P₁.

In yet another embodiment, which is illustrated in FIG. 5 b, threepoints p0, p1, p2 are joined by an arc. A distance d1 is the shortestdistance between a middle portion of line segment p0p1 and the arcp0p1p2; and a distance d2 is the shortest distance between the middleportion of line segment p1p2 and the arc p0p1p2. In one embodiment,distance d1 is the shortest distance between a midpoint of line segmentp0p1 and the arc p0p1p2; and a distance d2 is the shortest distancebetween the midpoint of line segment p1p2 and the arc p0p1p2. A middleone of the three points, i.e., point p1, may be marked for smoothing ifthe following three conditions or criteria are satisfied:

-   Test 1: Both p0p1 and p1p2 are lines.-   Test 2: Max{d1,d2}<0.0005 inch. That is, both d1 and d2 are less    than some predetermined distance approximately between 0.0002 and    0.001 inch, such as 0.0005 inch.-   Test 3: The angle θ>145 degrees, or some other predetermined angle    approximately between 130 degrees and 160 degrees.    If the three above conditions are not met, then point p1 may not be    marked for smoothing.

FIG. 18 illustrates a method 1800 of the present invention as describedabove with reference to FIG. 5 b. In a first step 1802, threeconsecutive points of a data path are identified. For example, pointsp0, p1 and p2 may be identified. In step 1804, a first line distancebetween a first of the points and a second of the points and/or a secondline distance between the second point and a third of the points areascertained. More particularly, a first line distance 518 between pointsp0 and p1, and a second line distance 520 between points p1 and p2 isascertained. In a next step 1806, a turn angle between a first lineextending between the first point and the second point and a second lineextending between the second point and the third point is determined. Inthe example of FIG. 5 b, a turn angle θ between the line extendingbetween points p0 and p1 and the line extending between points p1 and p2is determined. As indicated in steps 1808, 1810 and 1812, smoothing isperformed on the second point if the first line distance and/or thesecond line distance is less than a threshold line distance, and theturn angle is greater than a threshold turn angle. That is, smoothingmay be performed on point p1 if first line distance 518 and/or secondline distance 520 is less than a threshold line distance, and turn angleθ is greater than a threshold turn angle. Else, another threeconsecutive data points of the data path may be identified, as indicatedin step 1802. For example, consecutive points p1, p2 and a next point p3(not shown) may be identified.

Adjust for Smoothing

In a second operation, line data is adjusted for smoothing. The secondoperation may reduce the curvature changes on a three-dimensional curverepresented by a sequence of three-dimensional points. The localcurvature at a given point on such a curve is mainly determined byneighboring points. A minor change of the position of the given point orof its neighboring points may significantly change the local curvature.Thus, it is possible to modify the positions of points within the giventolerance such that the curvature changes on the three-dimensional curverepresented by the modified points will be reduced. The smaller thecurvature changes on the curve, the smoother the curve may be.

The second operation may include sequentially checking a group of fourconsecutive line target points already processed by the first operationin smoothing queue 206 and adjusting the positions of middle points tomodify the local curvature. Let p₁, p₂, p₃ and p₄ be four consecutiveline target points processed by the first operation in smoothing queue206. Assume that points p₂ and p₃ are both tagged “smoothing”, thenthese two middle points will be slightly moved. The algorithm foradjusting point positions as illustrated in FIG. 6 may include thefollowing steps: 1) create an arc ARC₁₃₄ passing through points p₁, p₃and p₄; 2) create an arc ARC₁₂₄ passing through points p₁, p₂ and p₄; 3)move point p₂ towards arc ARC₁₃₄ and keep the moving distance within thegiven tolerance; and 4) move point p₃ towards arc ARC₁₂₄ and keep themoving distance within the given tolerance.

The positional adjustment proceeds continuously with groups of fourconsecutive points.

After one group is adjusted, the successive group that consists of thelast three points in the previous group and a new target point of thenext line element will be processed. The target points of line elementsin smoothing queue 206 processed by the second operation are moved(within the given tolerance range) so that the curvatures at thesepoints change more smoothly, i.e., less drastically, than they didbefore the second operation.

Step 212 includes adjusting line moves for smoothing. The line moves maybe adjusted relative to each other to control the path curvature and toprepare for arc fitting. In the illustration in FIG. 6 a, twoconstruction arcs 602, 604 are located that pass through points P₀P₁P₃and P₀P₂P₃, respectively. Points P₁ and P₂ may both be moved into anarea between construction arcs 602, 604. Points P₁ and P₂ are movedcloser to one another such that point P₂ is moved approximately midwaybetween construction arcs 602, 604, and point P₁ is moved a lesserdistance toward construction arc 604. That is, point P₁ may not be movedso far as to be positioned midway between construction arcs 602, 604.Rather, point P₁ may be moved less than half of the distance to theopposite construction arc 604. More particularly, point P₁ may be moveda distance equal to a factor between 0 and 0.5 multiplied by a spacingS₁ (FIG. 7) between the point P₁ and the opposite construction arc 604.The factor may be less than 0.45. In the embodiment illustrated in FIG.6 a, the factor is approximately between 0.18 and 0.20, which has beenestablished mathematically as providing favorable results.

FIG. 7 is an enlarged view that illustrates with more precision thedirection and distance of the movement of points P₁ and P₂ in oneembodiment. A line 702 intersects point P₂ and is tangent to arc 604 atpoint P₂. A spacing S₂ is defined between point P₂ and arc 602 in adirection perpendicular to line 702. Point P₂ may be moved a distanceequal to a factor between 0.45 and 0.55 multiplied by spacing S₂ in adirection perpendicular to line 702. In the embodiment illustrated inFIGS. 6 a and 7, the factor is approximately 0.50. Similarly, a line 704intersects point P₁ and is tangent to arc 602 at point P₁. A spacing S₁is defined between point P₁ and arc 604 in a direction perpendicular toline 704. Point P₁ may be moved a distance of approximately 0.19multiplied times spacing S₁ in a direction perpendicular to line 704.

The process may then be repeated for each successive move in theprogrammed path. FIG. 8 illustrates a repetition of the process after anext point P₄ has been identified. Two construction arcs 802, 804 aredefined that pass through points P₁P₂P₄ and P₁P₃P₄, respectively. PointsP₂ and P₃ are then moved by factors of 0.19 and 0.50, respectively, in amanner substantially similar to the movements of points P₁ and P₂ inFIGS. 6 a and 7.

FIG. 6 b illustrates another embodiment of the present invention foradjusting line moves for smoothing. The line moves may be adjustedrelative to each other to control the path curvature and to prepare forarc fitting. Two construction arcs 612, 614, which are not coplanar, arelocated that pass through points P₀P₁P₃ and P₀P₂P₃, respectively. A lineof intersection of the planes of arcs 612, 614 is designated 616. Thecenter of a circle defined by arc 612 is designated c2; and the centerof a circle defined by arc 614 is designated c1. A projection of pointp1 on the plane of arc 614 is designated point q1. That is, a line thatextends through both of points p1 and q1 is perpendicular to the planeof arc 614. Similarly, a projection of point p2 on the plane of arc 612is designated point q2. That is, a line that extends through both ofpoints p2 and q2 is perpendicular to the plane of arc 612. The closestpoint on arc 614 to point p1 is designated s1; and the closest point onarc 612 to point p2 is designated s2.

Generally, a point is adjusted if both that point and an adjacent pointhave been marked for smoothing. A point may be marked for smoothing ifit satisfies each of the three tests or conditions given above, i.e.,both p0p1 and p1p2 are lines; Max {d1,d2}<0.0005 inch; and angle θ>143.2degrees. In the example shown in FIG. 6 b, point p1 may be adjusted ifp1 has been marked for smoothing and either or both of p0 and p2 hasbeen marked for smoothing. Point p1 may be adjusted, if at all, along anadjustment vector v1 extending from p1 to s1. Similarly, point p2 may beadjusted if p2 has been marked for smoothing and either or both of p1and p3 has been marked for smoothing. Point p2 may be adjusted, if atall, along an adjustment vector v2 extending from p2 to s2.

After the adjustments associated with arcs 612 and 614, the processcontinues with subsequent adjustments associated with arcs formed by thenext set of four points, i.e., points p1, p2, p3 and p4. That is,adjustments may be made based upon an arc defined by points p1, p2 andp4 and an arc defined by points p1, p3 and p4. As with the embodiment ofFIGS. 6 a and 7, the adjustment process may continue indefinitely withcalculations based upon subsequent sets of four points along thetrajectory.

The magnitude of the adjustment of a point's position along anadjustment vector may be dependent upon whether one of the two adjacentpoints is unmarked for smoothing. More particularly, if a marked pointlies immediately after or before an unmarked point, i.e., the markedpoint lies between an unmarked point and another marked point, then theposition of the marked point may be adjusted by half the length of theadjustment vector. That is, the marked point may be moved half way alongthe adjustment vector. In equation form, the position of the point maybe calculated as p=p+0.5*v. That is, the point is moved with acoefficient or factor of 0.5. Otherwise, if a marked point lies betweentwo other marked points, then the position of the point is adjustedtwice with a coefficient or factor of approximately 0.19 in each of thetwo adjustments. In the first adjustment, the direction and magnitude ofthe adjustment vector is determined with the adjusted point being thesecond of the four points used in the calculations illustrated in FIG. 6b. In the second adjustment, the direction and magnitude of theadjustment vector is determined with the adjusted point being the thirdof the four points.

Adjustment factors of 0.19 and 0.5 are discussed above in conjunctionwith the embodiment of FIG. 6 b. However, as with the embodiment ofFIGS. 6 a and 7, these factors may lie within respective ranges of 0.18to 0.20 and 0.45 to 0.55.

The total magnitude of the adjustment of a point may be limited by aconstant value. In one embodiment, the total distance of adjustment islimited to 0.0002 inch. If the calculated total adjustment is greaterthan 0.0002 inch, then the actual implemented adjustment is set to0.0002 inch in the direction of the calculated adjustment.

To better illustrate the embodiment of FIG. 6 b, various adjustmentscenarios are described below. The check to verify that the totaladjustment is less than an adjustment limit, such as 0.0002 inch, isomitted for simplicity and ease of illustration.

Case 1:

-   Condition: p0 is not marked for smoothing; p1 is marked for    smoothing; p2 is not marked for smoothing.-   Result: Because p1 is not adjacent another marked point, p1 is not    adjusted even though it is marked for smoothing.    Case 2:-   Condition: p0 is not marked for smoothing; p1 and p2 are marked for    smoothing; p3 is not marked for smoothing.-   Result: p1=p1+0.5*v1; p2=p2+0.5*v2.    Case 3:-   Condition: p0 is not marked for smoothing; p1, p2 and p3 are marked    for smoothing; p4 is not marked for smoothing.-   Result: p1=p1+0.5*v1; p2=p2+0.1877*v2+0.1877v2′; p3=p3+0.5*v3,    wherein v1 and v2 are calculated from points p0p1p2p3, and points    v2′ and v3 are calculated from points p1p2p3p4.    Case 4:-   Condition: p0 is not marked for smoothing; p1, p2, p3 and p4 are    marked for smoothing.-   Result: p1=p1+0.5*v1; p2=p2+0.1877*v2+0.1877v2′;    p3=p3+0.1877*v3+0.1877v3′, wherein v1 and v2 are calculated from    points p0p1p2p3, v2′ and v3 are calculated from points p1p2p3p4, and    v3′ is calculated from points p2p3p4p5.

FIG. 19 illustrates a method 1900 of the present invention as describedabove with reference to FIG. 6 b which represents a trajectory path of amotive body. In a first step 1902, four consecutive points of a datapath, including a first point, a second point, a third point and afourth point, are identified. For example, points p0, p1, p2 and p3 maybe identified. In step 1904, a first arc defined by the first point, thesecond point and the fourth point is located. In the illustration ofFIG. 6 b, a first arc 612 defined by points p0, p1 and p3 is identified.In step 1906, a second arc defined by the first point, the third pointand the fourth point is located. In FIG. 6 b, a second arc 614 definedby points p0, p2 and p3 is identified. In a final step 1908, the secondpoint is moved into an area between the first arc and the second arc.For example, point p1 may be moved along adjustment vector v1 into anarea between arcs 612 and 614.

Step 214 (FIG. 2) includes smoothing data using a double arc algorithm.Line moves may be converted into two arc moves that are tangent to eachother and that maintain a given path tolerance. As illustrated in FIG.9, a line move associated with points P₀ and P₁ may be replaced with twoarc moves associated with points P₀ and P₁. More particularly, a linemove 902 from point P₀ to point P₁ may be converted into Arc₁ from P₀ toarc junction point P′ and Arc₂ from arc junction point P′ to P₁. Arc₁and Arc₂ may be connected to each other and may be tangent to each otherat point P′. Arc₁ may be tangent to a preceding move 904 whichterminates at point P₀. Preceding move 904 may be a line move or an arcmove. Further, Arc₂ may be tangent to a succeeding or next move 906which begins at point P₁. Succeeding move 906 may be a line move or anarc move, and may be defined after the adjusted position of the nextpoint P₂ (not shown in FIG. 9) is determined. Smooth transitions betweenall the program moves are thus enabled. In one embodiment, a radius 908of Arc₁ is different from a radius 910 of Arc₂. Radii 908 and 910 areshown only partially in FIG. 9. Data smoothing essentially smoothes outabrupt turns or discontinuities in the tool path to provide a smoothertool motion.

FIG. 20 illustrates a method 2000 of the present invention as describedabove with reference to FIG. 9. In a first step 2002, a line moveextending from a first point of the data path to a second point of thedata path is identified. For example, line move 902 extending from pointP₀ to point P₁ may be identified. In a second step 2004, the line moveis replaced with a first arc move and a second arc move, the first arcmove beginning at the first point and terminating at a junction point,the second arc move beginning at the junction point and terminating atthe second point. In the illustration of FIG. 9, line move 902 isreplaced with arc moves Arc₁ and Arc₂, Arc₁ beginning at point P₀ andterminating at junction point P′, and Arc₂ beginning at junction pointP′ and terminating at second point P₁.

Double Arc Smoothing

In a third operation, smoothing is performed using a double arcalgorithm. The third operation may include converting the lines intoarcs, which are mutually tangent to each other, while maintaining agiven tolerance. This third operation may include sequentially checkingconsecutive line target points processed in the second operation insmoothing queue 206 and performing the conversion of lines to arcs.

The double arc algorithm is developed for the third operation. Thisalgorithm causes the final smoothed trajectory to pass through all thetarget points of the line segments to be smoothed. Thus, the tolerancerequirements will be well satisfied.

The double arc algorithm may perform two functions simultaneously in apipeline manner. First, the algorithm may determine a tangent vector ateach target point of a line segment to be smoothed. Second, thealgorithm may generate double arcs to replace the line segments to besmoothed. Let p₁, p₂, p₃, p₄, p₅, p₆, . . . , p_(n) be consecutive linetarget points already processed by the second operation in smoothingqueue 206. The first function may perform the following steps:

-   If p_(i), p_(i−1) or p_(i+1) is tagged “smoothing”, calculate a    desired tangent vector t_(i) (FIG. 29) at point p_(i) based on the    positions of points p_(i−1), p_(i), and p_(i−1). The three points    p_(i), p_(i−1) and p_(i+1) define the Arcp_(i−1)p_(i)p_(i+1) and    t_(i) is the tangent vector to the arc at point p_(i).-   If none of p_(i), p_(i−1), p_(i+1) is tagged “smoothing”, do nothing-   Increase i by 1-   Repeat the above steps

The above function will create a sequence of desired tangent vectors atthe target points of the line segments to be smoothed, denoted as t₁,t₂, t₃, t₄, t₅, t₆, . . . , t_(n). The tangen vectors may be referred toas “desired tangent vectors” because the direction changes of thesevectors are gradual, which further smoothes the curvature changes in thesmoothed trajectory.

The second function may perform the following steps:

-   Check points p_(i), and p_(i+1) to see if the corresponding desired    tangent vectors t_(i), and t_(i+1) have been calculated for p_(i),    and p_(i+1) or not-   If the tangent vectors t_(i), and t_(i+1) are both available, then    create a pair of three-dimensional arcs a_(i1) and a_(i2) such that:    -   a_(i1) starts at point p_(i) and is tangent to the tangent        vector t_(i)    -   a_(i1) is tangent to a_(i2) at a point p between p_(i) and        p_(i+1)    -   a_(i2) starts at point p, ends at point p_(i+1), and is tangent        to the tangent vector t_(i+1)-   If any of the tangent vectors t_(i), and t_(i+1) is not available,    do nothing-   Increase i by 1-   Repeat the above steps

In another embodiment, the second function may perform the followingsteps:

-   If p_(i), p_(i+1) and p_(i+2) are marked for smoothing and the    corresponding desired tangent vectors t_(i), t_(i+1) and t_(i+2)    have been calculated for p_(i), p_(i+1) and p_(i+2), then create a    pair of three-dimensional arcs a_(i1) and a_(i2) such that:    -   a_(i1) starts at point p_(i) and is tangent to the tangent        vector t_(i)    -   a_(i1) is tangent to a_(i2) at a point p′ between p_(i) and        p_(i+2)    -   a_(i2) starts at point p′, ends at point p_(i+2), and is tangent        to the tangent vector ti+₂.-   Check the shortest distance (d) between p_(i+1) and the closest arc.    If the distance is within a given tolerance, the double arcs are    valid.-   If the double arcs between p_(i) and p_(i+2) are invalid, then    create a pair of three-dimensional arcs a_(i1) and a_(i2) between    p_(i) and p_(i+1) such that:    -   a_(i1) starts at point p_(i) and is tangent to the tangent        vector t_(i)    -   a_(i1) is tangent to a_(i2) at a point p′ between p_(i) and        p_(i+1)    -   a_(i2) starts at point p′, ends at point p_(i+1), and is tangent        to the tangent vector t_(i+1)-   Repeat the above steps to generate double arcs between p_(i+1) and    p_(i+2)-   Increase i by 2-   Repeat the above steps

The operation of the second function in the embodiment described aboveis illustrated in FIGS. 30 and 31. In FIG. 30, p0, p1 and p2 are markedfor smoothing. A pair of three-dimensional arcs a1 and a2 are createdsuch that arc a1 starts at point p0 and is tangent to tangent vector t0;arc a1 is tangent to arc a2 at a point p′ between p0 and p2; and arc a2starts at point p′, ends at point p2, and is tangent to the tangentvector t2. If a distance d between p1 and the closer arc a1 of the twoarcs is within a given or predetermined tolerance, then the double arcsa1 and a2 are valid.

If, on the other hand, the double arcs a1 and a2 are invalid, i.e., thedistance d is not within the given tolerance, then another pair ofthree-dimensional arcs a3 and a4 (FIG. 31) are created such that arc a3starts at point p0 and is tangent to tangent vector t0; arc a3 istangent to arc a4 at a point p″ between p0 and p1; and arc a4 starts atpoint p″, ends at point p1, and is tangent to the tangent vector t1. Theabove steps may then be replicated in order to create yet another pairof three-dimensional arcs (not shown) between points p1 and p2. Any orall of arcs a1, a2, a3 and a4 may be circular arcs, and each of the arcsmay have a different, respective radius.

With the double arc smoothing thus completed between points p0, p1 andp2, the above-described second function may be repeated for the nextthree points along the trajectory, i.e., points p2, p3 and p4. Further,the second function may be repeated for each subsequent set of threepoints along the trajectory.

The above-described double arc smoothing embodiment is directed to thecase in which p0, p1 and p2 are each marked for smoothing. Assume nowthat, of the four points p0, p1, p2 and p3, p1, p2 and p3 are marked forsmoothing. Tangential vectors t0 and t1 may be derived from the arcdefined by p0, p1, p2, and tangential vector t2 may be derived from thearc defined by p1, p2, p3. The following five steps may be taken:

-   -   1. Double arcs, i.e., arc a1 from p0 to p′ and arc a2 from p′ to        p2, which are tangential at point p′ may be generated from (p0,        p2, t0, t2), as shown in FIG. 30.    -   2. The shortest distance between p1 and the closer of the two        double arcs may then be checked. If the distances in the x, y        and z directions are less than a predetermined distance, such as        0.0002 inch, then the double arcs a1 and a2 are valid.    -   3. If the double arcs a1 and a2 are invalid, then double arcs a3        and a4 are generated between points p0 and p1 and based upon        tangent vectors t0, t1. The two arcs a1 and a2 may be tangential        at point p″.    -   4. There may be a tolerance check for the double arcs. A        distance d in FIG. 31 is the shortest distance between point p″        and a line extending between points p0 and p1. If distance d is        less than a predetermined distance, such as 0.0005 inch, then        the double arcs a3 and a4 are valid. Otherwise, if the double        arcs a3 and a4 are invalid, then define the trajectory between        points p0, p1 as a line extending between points p0 and p1 .    -   5. Repeat steps 3 and 4 to generate double arcs between p1 and        p2 based upon tangent vectors t1 and t2.

The double arc algorithm replaces a line segment with a pair ofthree-dimensional arcs. Adjacent arcs created by the double arcalgorithm may be tangent to each other, and all arcs may have reducedcurvature changes. The double arc algorithm may be especially suitablefor use in conjunction with TBC, which would yield several advantages.First, for most three-dimensional surfaces and curves, the curvatureconstantly changes. Mutually tangent shorter arcs provide better andsmoother approximation than longer arcs. Second, for five or six axesmachining, short three-dimensional XYZ arcs can coordinate withpoint-to-point moves in ABC axis space. This is a significant advantagesince it is difficult to extend the non-TBC long arc three-dimensionalsmoothing to five or six axes machining. Third, all thethree-dimensional arc commands will be directly executed by TBC usingtrue arc technology, which controls motion along the true arc trajectorywithout polygon approximation. After smoothing, the elements insmoothing queue 206, including positioning commands, line commands, arccommands, and the program end command, may be sent to lookahead queue208.

FIG. 21 illustrates a method 2100 of the present invention as describedabove with reference to FIGS. 5 b, 6 b, 30 and 31. In a first step 2102,four consecutive points of the path data, including a first point, asecond point, a third point and a fourth point, are identified. In theembodiment described above with reference to FIG. 6 b, four consecutivepoints p0, p1, p2 and p3 are identified. In a second step 2104, it isdetermined whether the second point is suitable for smoothing. Forexample, it may be determined, according to the method described abovewith reference to FIG. 5 b, whether point p1 is suitable for smoothing.If not, operation may return to step 2102 wherein another set of fourconsecutive points is identified. For example, four consecutive pointsp1, p2, p3 and p4 may be identified. If the second point is suitable forsmoothing, then a first arc defined by the first point, second point,and fourth point is located (step 2106). In the embodiment of FIG. 6 b,arc 612 is identified. In a next step 2108, a second arc defined by thefirst point, third point, and fourth point is located. In the embodimentof FIG. 6 b, arc 614 is identified. Next, in step 2110, the second pointis moved into an area between the first arc and the second arc. Forexample, as indicated in FIG. 6 b, point p1 may be moved alongadjustment vector v1 into an area between arcs 612 and 614. Lastly, instep 2112, a line move associated with the second point may be replacedwith at least one arc move associated with the second point. Forexample, a line move leading into point p1, or a line move originatingfrom point p1, may be replaced with an arc move, as illustrated in FIGS.30 and 31.

Additive Lookahead

In an additive lookahead operation within lookahead queue 208, it isensured that there is enough distance to accelerate or deceleratebetween all velocity changes in the program data. For each programmedmove, a stop distance is computed that defines the distance required todecelerate the axes to zero velocity according to S-curve acceleration.It is referred to as S-Curve because the acceleration/deceleration partof the velocity profile (velocity plotted versus time) illustrated inFIG. 10 looks like an “S” instead of a straight line. The stop distancemay be determined by the lower of: a) the maximum move velocitydetermined by the angle between the current and next moves; and b) themaximum move velocity for the path curvature determined by the machinemodel.

The Lookahead operation may ensure enough distance to accelerate ordecelerate between all velocity changes. This has advantages in that themachine bumping and dwell marks may be reduced, and the surface finishin machining may be improved.

Three functions may be planned for the Lookahead operation, namely,third order smoothing, pre-lookahead, and additive lookahead. Theadditive lookahead function may include searching in lookahead queue 208for velocity and distance information and determining whether there isenough distance for the machine to accelerate/decelerate properly fromone motion command to the next. If there is not enough distance, thenthe information may be stored and used in the motion kernel to reducethe velocity so that the machine may accelerate/decelerate properly.

There is a mathematical conversion between the stop distance and thevelocity. The proper velocity may be calculated from the stop distanceand the S-Curve, as discussed in more detail below.

The additive lookahead algorithm may require knowledge of the maximumvelocities allowed for different part geometries. A tolerance/arcradius/velocity table may be built for this purpose. The table mayprovide the maximum allowable velocities for arcs of different radii orcurvatures with different tolerance requirements.

The additive lookahead function may check each newly coming motioncommand at the head of lookahead queue 208 and calculate the stopdistance for the previous command in the following steps:

-   Compare the previous command with the command preceding the previous    command to get the angle or curvature between these two commands,    and use this angle to determine the maximum joint-point velocity    which is the allowable maximum velocity at the joint point of these    two consecutive motion commands.-   Look up the tolerance/arc radius/velocity table. According to the    motion trajectory and the required tolerance, determine the maximum    allowable trajectory-velocity for the previous command's motion    trajectory.-   Compute the stop distance for the previous command by taking the    smaller of the maximum joint-point velocity and the maximum    trajectory-velocity, and convert this smaller velocity to the stop    distance according to the S-curve velocity profile.-   Attach the calculated stop distance to the previous command.

The lookahead is a dynamic process. The continuous trajectory changesmay affect the previously processed commands. Each time a new command isadded to lookahead queue 208 and a stop distance is calculated for thenew command, all the commands in the queue which have already beenprocessed may be reviewed to determine whether the previously calculatedstop distances for these commands need to be updated or not. After a newmove is processed and placed in a Lookahead queue 208, all previousmoves in queue 208 may be reviewed and may have their velocitiesadjusted downward if required. This final preprocessing operation isanalogous to placing speed limits signs along a road before a curve sothat the driver can safely navigate through the curve.

The lookahead process includes a first step 216 of determiningjoint/path feedrate changes, and a second step 218 of looking ahead forthe stop distance. In step 216, feedrate changes along the path frompoint P_(i−1) to point P_(i) are determined. In step 218, a segmentlength L_(i) may be determined between points P_(i−1) and P_(i), asillustrated in FIG. 11. Stop distances may also be determined for eachof points P_(i−1) and P_(i). The stop distance at point P_(i−1) shouldnot be greater than the sum of L_(i) and the stop distance at pointP_(i). Else, it may not be possible to comply with velocity constraintsbeyond point P_(i). It is this sum to which the term “additive” in“additive lookahead” refers. If it is found that the stop distance atpoint P_(i−1) is greater than the sum of L_(i) and the stop distance atpoint P_(i), then the velocity at point P_(i−1) may be reduced such thatthe stop distance at point P_(i−1) is equal to the sum of L_(i) and thestop distance at point P_(i). In equation form,if (Stop distance_(i)+ΣL_(i))<Stop distance_(i−1), thenset (Stop distance_(i+ΣL) _(i))=Stop distance_(i−1).

In another form, let cmd₁, cmd₂, cmd₃, cmd₄, cmd₅, cmd₆, , cmd_(i) beconsecutive commands in lookahead queue 208 which have been processed.For command cmd_(j), 0<j<i+1, let its path length be len, and the stopdistance be stopdis_(j). A “valid stop distance condition” is defined asfollows: If the stop distance stopdis_(j) for the command cmdj is equalto or less than the sum of the path length len_(j+1) and the stopdistance stopdis_(j+1) for the command cmd_(j+1), then the commandcmd_(j) meets the “valid stop distance condition”.

When a new command cmd_(i+1) is received and processed, it has a pathlength len_(i+1), and a stop distance stopdis_(i+1) Let j=i. Theadditive lookahead algorithm may perform the following steps:

-   Check command cmd_(j) to determine whether it meets the “valid stop    distance condition”, i.e., whether stopdis_(j) is equal to or less    than the sum of len_(j+1) and stopdis_(j+1)-   If command cmd_(j) does not meet the “valid stop distance    condition”, i.e., stopdis_(j) is greater than the sum of len_(j+1)    and stopdis_(j+1), then the stop distance for command cmd_(j) will    be replaced by the sum of len_(j+1) and stopdis_(j+1)-   If command cmd_(j) meets the “valid stop distance condition”, i.e.,    stopdis_(j) is equal to or less than the sum of len_(j+1) and    stopdis_(j+1), then stop distance stopdis_(j) for command cmd_(j)    may remain unchanged, and the update operation is completed-   Ifj is equal to 0, the update operation is completed-   Set j=j−1, i.e., decrease j by 1-   repeat the above steps

When the additive lookahead function is, in a backward direction,sequentially reviewing the commands previously processed in lookaheadqueue 208, checking the “valid stop distance condition”, and updatingthe stop distance of the commands, the updating operation may stop afterthe first command that meets the “valid stop distance condition” isfound. Because the command meets the “valid stop distance condition”,the stop distance of the command may remain unchanged. Thus, all thecommands previous to the command were met and are still meeting the“valid stop distance condition”. That is, nothing changed and nothingneeds to change. This is an advantageous property of the additivelookahead algorithm, which provides great efficiency for lookaheadoperation. Instead of looking back and updating the whole lookaheadqueue 208, only a few elements in the queue need to be updated, onaverage.

Stop Distance and Stopper

The additive lookahead algorithm introduces the concepts of stopdistance and stopper. The stop distance of a motion command with nominalfeedrate v₁ (FIG. 32 a) and end point speed v₀ may be defined as thedistance traveled from a target point (FIG. 32 b) to the point at whicha full stop can be achieved according to the S-curve speed profile, asillustrated by the shaded area in FIG. 32 a. Stopper, which is a virtualtarget point for a motion command, may be selected such that if thesystem reduces speed from nominal feedrate v₁ and comes to a full stopat the stopper according to the S-curve, it passes the actual targetpoint at the desired end point speed v₀. The stop distance may bedefined as the distance between the target point and the stopper.

Once the nominal feedrate v₁ of a motion command is determined, thereexists a one-to-one mapping between the stop distance and the end pointspeed v₀, assuming that the S-curve speed profile is fixed, i.e., themaximum acceleration and jerk are fixed. It may be assumed herein,unless otherwise stated, that the S-curve profile is fixed. Therefore,the stop distance has a direct correspondence to the end point speed;and if the stop distance is given, the end point speed may beaccordingly uniquely determined, and vice versa. The additive lookaheadalgorithm may use the stop distance as a metric of end point speed,which allows the algorithm to be computationally efficient, as discussedbelow.

The stopper may be used in an S-curve controller to design a motionprofile. The S-curve controller may aim at the stopper instead of at anactual target point. The S-curve controller may generate a speed profilein which the controlled tool passes the actual target point at the samepoint in time at which the tool reaches the desired slowdown speed.

Speed Limitations of Motion Commands

In the additive lookahead algorithm, each motion command may have fourspeed limitations: specified feedrate, machine allowable speed, pathvelocity limit and joint velocity limit. The specified feedrate may bedefined as the desired feedrate value given by the upper-level motionplanning. The machine allowable speed is a machine limitation on thespeed. The maximum allowable speed depends on the hardware of themachine and laws of physics. The path velocity limit is a limit for arcmotion. The path velocity limit may depend on the arc radius andtolerance specification. The larger the arc radius and tolerance are,the larger the maximum path velocity limit is. The joint velocity limitmay be defined as a speed limit at the joint of two consecutive motioncommands. The joint velocity limit may depend on the angle between thetwo motions and the tolerance specification. The larger the toleranceand the smaller the angle are, the larger the joint speed limit valueis.

In the additive lookahead algorithm, the nominal feedrate is the lowestvalue among the specified feedrate, the machine allowable speed and thepath velocity limit. For example, if the specified feedrate is higherthan the machine allowable speed and the path velocity limit, thelookahead algorithm would force the feedrate to slow down to the smallervalue of the machine allowable speed and the path velocity limit.

The slowdown feedrate at the end point of a motion command may depend onthe joint velocity limit as well as the nominal feedrate of that motioncommand and the next motion command. Because the nominal feedrate is thesmallest one among the specified feedrate, the machine allowable speedand the path velocity limit, slowdown may occur during a current motionsegment and speedup may occur in a next motion segment, such that thefeedrate for the current motion segment may be less than the nominalvalue. The additive lookahead algorithm may use the nominal and slowdownfeedrates to calculate the stop distance.

Valid Stop Distance Condition

The lookahead algorithm may ensure that the distance between the tooland some location ahead of the tool is sufficient to allow the speed ofthe tool to be changed to a desired level by the time the tool arrivesat the location. This condition may be referred to as a “valid stopdistance condition” with regard to the additive lookahead algorithm andmay be defined mathematically by equation (1):ds _(i) ≦l _(i+1) +ds _(i+1)  (1)where ds and l represent the stop distance and the length of a motionsegment, respectively, and subscripts i and i+1 represent the index ofmotion commands. In the diagram of FIG. 33, motion command cmd_i has anend point at p_i, stopper point at stopper_i, and stop distance ds_i. Ifthe stop distance of cmd_i does not exceed the sum of the length andstop distance of cmd_(i+1), or if the stopper_i is in front of thestopper_(i+1), then cmd_i satisfies the valid stop distance condition.

The valid stop distance condition can also be explained in another wayas illustrated by equation (2):ds _(i) −ds _(i+1) ≦l _(i+1)  (2)Because the stop distance ds_i and ds_(i+1) correspond to the feedrateat the end point p_i of motion cmd_i and the feedrate at the end pointp_(i+1) of motion cmd_(i+1) respectively, equation (2) indicates thatthe segment length l_(i+1) is long enough to accommodate a speedslowdown from the feedrate at the point p_i to the feedrate at the pointp_(i+1).Additive Lookahead vs. Non-additive Lookahead

A difference between a TBC additive lookahead algorithm and anon-additive lookahead algorithm is that the additive lookaheadalgorithm generates an S-curve targeting the stopper and uses only partof the S-curve so that the speed is reduced from v₁ to v₀ at the targetpoint. The non-additive lookahead generate an S-curve targeting theactual target point and the speed is reduced from v₁ to v₀ at the targetpoint.

FIG. 34 shows a comparison between the additive and non-additive S-curvespeed profile. The speed is reduced from v₁ to v₀ at the target point inboth profiles. However, in the additive S-curve, the slowdown occursbetween time t_(tbc1) and time t_(tbc0), while in the non-additiveS-curve, the slowdown occurs between time t_(wm1) and time t_(wm0).Thus, the non-additive S-curve requires a longer time and a longerdistance than the additive S-curve in order to achieve the samereduction in speed from v₁ to v_(0.)

Additive lookahead may be a dynamic process. The continuous trajectorychanges may affect the previously-processed commands. Each time a newcommand is added to the lookahead queue and a stop distance iscalculated for the new command, all the commands in the queue which havealready been processed may be reviewed to see if thepreviously-calculated stop distances for these commands need to beupdated or not.

When a new command cmdi is received and processed, it has a path lengthl_i, and a stop distance d_i. The additive lookahead algorithm mayperform the following steps:

-   Check command cmd_(i−1) to see if it meets the “valid stop distance    condition”, i.e., if ds_(i−1) is equal to or less than the sum of    l_i and d_i.-   If command cmd_(i−1) does not meet the “valid stop distance    condition”, i.e., ds_(i−1) is greater than the sum of l_i and d_i,    then replace the stop distance ds_(i−1) for command cmd_(i−1) with    the sum of l_i and d_i.-   If command cmd_(i−l) meets the “valid stop distance condition”,    i.e., ds_(i−1) is equal to or less than the sum of l_i and d_i, then    the stop distance ds_(i−1) for command cmd_(i−1) may remain    unchanged, and the update operation is complete.-   If (i−1) is equal to 0, the update operation is complete.-   i—(decrement the value of i) and repeat the above steps.

When the additive lookahead function is sequentially reviewing backwardthe commands previously processed in the lookahead queue, checking the“valid stop distance condition”, and updating the stop distance of thecommands, the updating operation may stop after the first command thatmeets the “valid stop distance condition” is found. Because this commandmeets the “valid stop distance condition”, the stop distance of thiscommand may remain unchanged. Further, because this command meets the“valid stop distance condition”, all the commands previous to it met,and still meet, the “valid stop distance condition”, and none of theprevious commands may need to change.

FIGS. 35 a-b provide a comparison of the additive lookahead and thenon-additive lookahead algorithms, respectively. In the additivelookahead (FIG. 35 a), only the third step, i.e., the step ofcalculating the stop distance ds_i, may require a lot of computation,and may be calculated as shown in equation (3). This calculation may beneeded only once for every motion command, and there may be no need torepeat it. $\begin{matrix}{{ds} = \left\{ \begin{matrix}{\frac{1}{6}{J\left( \frac{2v_{0}}{J} \right)}^{\frac{3}{2}}} & {v_{0} < {\frac{1}{2}v_{1}}} \\{{v_{1}\sqrt{\frac{v_{1}}{J}}} + {\frac{1}{6}{J\left( \frac{2\left( {v_{1} - v_{0}} \right)}{J} \right)}^{\frac{3}{2}}} - {\left( \frac{2\left( {v_{1} - v_{0}} \right)}{J} \right)^{\frac{1}{2}}v_{1}}} & {v_{0} \geq {\frac{1}{2}v_{1}}}\end{matrix} \right.} & (3)\end{matrix}$

In the non-additive lookahead algorithm (FIG. 35 b), both the third andfifth steps, i.e., the steps of calculating slowdown distance and thestep of calculating a speed reduction for command i−1, may require a lotof computation, and may be calculated as shown in equations (4) and (5),respectively. The “distance to slowdown”, equation (4), is similar tothe “calculate stop distance” equation (3). $\begin{matrix}{d_{stop} = \left\{ \begin{matrix}{{v_{0}T_{1}} + \frac{v_{0}v_{d}}{a_{\max}} + \frac{v_{d}T_{1}}{2} + \frac{v_{d}^{2}}{2a_{\max}}} & {v_{d} > {a_{\max}T_{1}}} & \left( {{general}\quad{case}} \right) \\{\left( {{2v_{0}} + v_{d}} \right)T_{1}} & {v_{d} = {a_{\max}T_{1}}} & \left( {{critical}\quad{point}} \right) \\{\left( {{2v_{0}} + v_{d}} \right)\sqrt{\frac{v_{d}T_{1}}{a_{\max}}}} & {v_{d} < {a_{\max}T_{1}}} & \left( {{degenerate}\quad{case}} \right)\end{matrix} \right.} & (4)\end{matrix}$

The “reduce speed for command i−1”, equation (5) calculates maximumallowable speed change to v₀ in the given distance, which may involvelarge computations such as in solving cubic equations. $\begin{matrix}{v_{d} = \left\{ \begin{matrix}\frac{\begin{matrix}{\sqrt{\quad{\left( {{2\quad v_{\quad 0}}\quad - \quad{a_{\quad\max}\quad T_{\quad 1}}} \right)^{2}\quad + \quad{8\quad a_{\quad\max}\quad d}}} -} \\\left( {{2v_{0}} + {a_{\max}T_{1}}} \right)\end{matrix}}{2} & {d > {{a_{\max}T_{1}^{2}} + {2v_{0}T_{1}}}} & \left( {{general}\quad{case}} \right) \\\begin{matrix}{a_{\max}T_{1}} \\{{positive}\quad{real}\quad{solution}\quad{of}}\end{matrix} & {d = {{a_{\max}T_{1}^{2}} + {2v_{0}T_{1}}}} & \left( {{critical}\quad{point}} \right) \\{{v_{d}^{3} + {4v_{0}v_{d}^{2}} + {4v_{01}^{2}v_{d}} - \frac{a_{\max}d^{3}}{T_{1}}} = 0} & {d < {{a_{\max}T_{1}^{2}} + {2v_{0}T_{1}}}} & \left( {{degenerate}\quad{case}} \right)\end{matrix} \right.} & (5)\end{matrix}$

Furthermore, the two steps of the non-additive algorithm represented byequations (4) and (5) may need to be performed recursively until thereis enough distance to slow down. Because the non-additive lookaheadalgorithm needs a longer distance to slow down a same amount of speed,as shown in FIG. 34, the loop has to be repeated more times than theadditive lookahead algorithm until enough distance is satisfied.

In general, the additive lookahead may be more computationally efficientthan the non-additive lookahead due to the additive lookahead havingonly one heavy load step compared with two heavy load steps in thenon-additive lookahead. Another reason the additive lookahead may bemore computationally efficient than the non-additive lookahead is thatthe additive lookahead's heavy load step is outside the recursive loopand is only performed once. In contrast, the non-additive lookaheadperforms the two heavy load steps recursively until enough distance isvalid. The additive lookahead also needs less distance to reduce a sameamount of speed, as shown in FIG. 34, which means fewer previous motioncommands need to be modified.

Kinematics Comparison of Additive and Non-Additive Lookahead Algorithms

Assume that two consecutive motion commands cmd_(i−1) and cmd_i havenominal feedrates v_(i−1) and v_i, respectively. At the joint of the twomotions, the feedrate has to slow down to v₀. FIG. 36 compares thekinematical characteristics of the non-additive lookahead algorithm andthe TBC additive lookahead algorithm.

Lookahead operations ensure enough distance to accelerate or deceleratebetween all speed changes and may be applied to all real time motioncontrol and trajectory planning, such as CNC machine tools, robotics,autonomous vehicles and so on. Because lookahead is usually performed inreal time and may modify previously processed motion commandsrecursively, the efficiency of the algorithm may be of importance to thereduction of the CPU load and the improvement of machine performance.

Non-additive lookahead algorithms may calculate the distance needed toaccelerate/decelerate from one speed to another according to some speedprofiles, such as the S-curve speed profile. When the distance of amotion segment is short and is not great enough to accommodate a largespeed change, lookahead algorithms may calculate and limit the speed ofthat motion. The calculation of the speed limit involves a large amountof computation, and repeatedly performing the calculation is notdesirable because it takes a lot of processing time and resources.

The additive lookahead algorithm may use a stop distance concept as ametric of speed limitation. Updating the speed limit in the additivelookahead is only one addition compared with the large amount ofcomputation involving solving cubic equations in non-additive lookaheadalgorithms. Overall, the computational efficiency, less iteration ofmodifying previous motions, and increased system throughput provide theadditive lookahead algorithm of the present invention with severaladvantages over non-additive lookahead algorithms.

FIG. 22 illustrates a method 2200 of the present invention as describedabove with reference to FIG. 11. In a first step 2202, a plurality ofpoints along a target tool path are identified. In the example of FIG.11, points P_(i−2), P_(i−1) and P_(i) along a target tool path areidentified. Next, in step 2204, a maximum allowable stopping distance ofthe tool at a last of the points is determined. In FIG. 11, segmentlength L_(i+1) may represent a maximum allowable stopping distance atlast point P_(i). Point P_(i+1) may not have advanced far enough downthe data pipeline for it to be identified, and thus its characteristicsmay not be known. That is, the location of point P_(i+1) and whether itis a corner point or a point at the end of the target tool path may notbe known. However, a minimum allowable distance L_(i+1) between pointsP₁ and P_(i+1) is known. Because point P_(i+1) may be a corner point ora point at the end of the target tool path, it may be necessary for thetool to come to a stop, or nearly come to a stop, at point P_(i+1).Thus, in order to ensure that it is possible for the tool to come to astop upon reaching point P_(i+1), the maximum allowable stoppingdistance of the tool at last point P_(i) is determined to be equal tothe minimum allowable distance L_(i+1) between points P_(i) and P_(i+1).In a next step 2206, a last maximum allowable speed of the tool at thelast point such that the tool may come to a stop within the maximumallowable stopping distance is ascertained. In one embodiment, theS-curve of FIG. 10 may be used to relate the maximum allowable stoppingdistance Li+₁ to the maximum speed at last point P_(i) at which it maybe possible for the tool to come to a stop upon reaching point P_(i+1),i.e., the last maximum allowable speed. In a final step 2208, an othermaximum allowable speed of the tool at an other one of the points suchthat the tool may possibly slow down to the last maximum allowable speedupon arriving at the last point is established. In the example of FIG.11, established is an other maximum allowable speed at an other pointP_(i−1) such that the tool may possibly slow down to the last maximumallowable speed at point P_(i) which was ascertained in step 2206. Inone embodiment, the S-curve of FIG. 10 may be used to relate thedistance L_(i) to the maximum speed at other point P_(i−1) at which itmay be possible for the tool to come slow down to the last maximumallowable speed upon reaching point P_(i).

Taking the example above further, the maximum allowable speed at otherpoint P_(i−1) may be compared to a maximum allowable tolerance speed atother point P_(i−1), i.e., the maximum speed at other point P_(i−1) atwhich it can be ensured that the actual tool path will remain withintolerance given the curvature of the target tool path at other pointP_(i−1). The lesser of the maximum allowable speed at other pointP_(i−1) and the maximum allowable tolerance speed at other point P_(i−1)may be identified, and the tool may be moved at this lesser speed atother point P_(i−1).

Taking the above example yet further, a preceding maximum allowablespeed of the tool at a point P_(i−2) preceding other point P_(i−1) suchthat the tool may possibly slow down to the lesser speed upon arrivingat other point P_(i−1) may be established. A maximum allowable tolerancespeed of the tool at preceding point P_(i−2) may be determined dependentupon the maximum allowable level of deviation and/or the curvature ofthe target tool path at preceding point P_(i−2). A lesser of the maximumallowable tolerance speed of the tool at the preceding point and thepreceding maximum allowable speed at the preceding point may beidentified.

FIG. 23 illustrates another method 2300 of the present invention asdescribed above with reference to FIG. 11. In a first step 2302, aplurality of points along a target tool path are identified. In theexample of FIG. 11, points P_(i−2), P_(i−1) and P_(i) along a targettool path are identified. Next, in step 2304, a maximum allowable levelof deviation of an actual tool path from the target tool path isestablished. For example, depending upon the requirements of the workpiece, 0.1 cm might be established as the maximum allowable level ofdeviation of the actual tool path from the target tool path. In step2306, a curvature of the target tool path is ascertained at a pluralityof locations along the target tool path. For example, an angle 1907(FIG. 11) between the line move from point P_(i−2) to point P_(i−1) andthe line move from point P_(i−1) to point P_(i) may be ascertained atpoint P_(i−1). In step 2308, a maximum allowable tolerance speed of thetool at each of the points along the target tool path is determined. Themaximum allowable tolerance speed is dependent upon the maximumallowable level of deviation and the curvature. For example, given theangle 2307, a maximum allowable tolerance speed at point P_(i−1) may bedetermined such that the actual tool path does not deviate from thetarget tool path by more than 0.1 cm. Next, in step 2310, a maximumallowable stopping distance of the tool at a last of the points isdetermined. In FIG. 11, segment length L_(i+1) may represent a maximumallowable stopping distance at last point P_(i). Point P_(i+1) may nothave advanced far enough down the data pipeline for it to be identified,and thus its characteristics may not be known. That is, the location ofpoint P_(i+1) and whether it is a corner point or a point at the end ofthe target tool path may not be known. However, a minimum allowabledistance L_(i+1) between points P_(i) and P_(i+1) is known. Becausepoint P_(i+1) may be a corner point or a point at the end of the targettool path, it may be necessary for the tool to come to a stop, or nearlycome to a stop, at point P_(i+1). Thus, in order to ensure that it ispossible for the tool to come to a stop upon reaching point P_(i+1), themaximum allowable stopping distance of the tool at last point P_(i) isdetermined to be equal to the minimum allowable distance L_(i+1) betweenpoints P_(i) and P_(i+1). In a next step 2312, a last maximum allowablestopping speed of the tool at the last point such that the tool may cometo a stop within the maximum allowable stopping distance is ascertained.In one embodiment, the S-curve of FIG. 10 may be used to relate themaximum allowable stopping distance L_(i+1) to the maximum speed at lastpoint P_(i) at which it may be possible for the tool to come to a stopupon reaching point P_(i+1), i.e., the last maximum allowable speed. Instep 2314, an other maximum allowable stopping speed of the tool at eachof the points other than the last point such that the tool may possiblyslow down to the last maximum allowable speed upon arriving at the lastpoint is established. In the example of FIG. 11, established is an othermaximum allowable stopping speed at an other point P_(i−1) such that thetool may possibly slow down to the last maximum allowable speed at pointP_(i) which was ascertained in step 2312. In one embodiment, the S-curveof FIG. 10 may be used to relate the distance L_(i) to the maximum speedat other point P_(i−1) at which it may be possible for the tool to comeslow down to the last maximum allowable speed upon reaching point P_(i).In a final step 2316, a lesser of the maximum allowable tolerance speedand the maximum allowable stopping speed is identified at each of thepoints. For example, the lesser of the maximum allowable tolerance speeddetermined in step 2308 and the maximum allowable stopping speedestablished in step 2314 may be identified at each of points P_(i−2),P_(i−1) and P_(i).

FIG. 24 illustrates yet another method 2400 of the present invention asdescribed above with reference to FIG. 11. In a first step 2402, aplurality of points along a target tool path are identified. In theexample of FIG. 11, points P_(i−2), P_(i−1) and P_(i) along a targettool path are identified. Next, in step 2404, a maximum allowablestopping distance of the tool at a last of the points is determined. InFIG. 11, segment length L_(i+1) may represent a maximum allowablestopping distance at last point P_(i). Point P_(i+1) may not haveadvanced far enough down the data pipeline for it to be identified, andthus its characteristics may not be known. That is, the location ofpoint P_(i+1) and whether it is a corner point or a point at the end ofthe target tool path may not be known. However, a minimum allowabledistance L_(i+1) between points P_(i) and P_(i+1) is known. Becausepoint P_(i+1) may be a corner point or a point at the end of the targettool path, it may be necessary for the tool to come to a stop, or nearlycome to a stop, at point P_(i+1). Thus, in order to ensure that it ispossible for the tool to come to a stop upon reaching point P_(i+1), themaximum allowable stopping distance of the tool at last point P_(i) isdetermined to be equal to the minimum allowable distance L_(i+1) betweenpoints P_(i) and P_(i+1). In a next step 2406, a maximum allowable speedof the tool at an other one of the points such that a required stoppingdistance of the tool at the other point is equal to or less than a sumof the maximum allowable stopping distance at the last point and thesegment length between the other point and the last point. In theembodiment of FIG. 11, a maximum allowable speed of the tool at pointP_(i−1) may be ascertained such that a required stopping distance of thetool at point P_(i−1) is equal to or less than a sum of the maximumallowable stopping distance at last point P_(i) and segment length L_(i)between other point P_(i−1) and last point P_(i). In one embodiment, theS-curve of FIG. 10 may be used to relate the required stopping distanceof the tool at point P_(i−1) to the maximum allowable speed at pointP_(i−1).

Taking the above example further, a preceding maximum allowable speed ofthe tool at a point P_(i−2) preceding other point P_(i−1) such that arequired stopping distance of the tool at the preceding point is equalto or less than a sum of the maximum allowable stopping distance at lastpoint P_(i) and segment length L_(i)+L_(i−1) between preceding pointP_(i−1) and last point P_(i). A maximum allowable tolerance speed of thetool at preceding point P_(i−2) may be determined dependent upon themaximum allowable level of deviation and/or the curvature of the targettool path at preceding point P_(i−2). A lesser of the maximum allowabletolerance speed of the tool at the preceding point and the precedingmaximum allowable speed at the preceding point may be identified.

The Queue Manager Task may be performed by a general queue manager in aqueue module that manages all the operations performed on tolerancequeue 202, compression queue 204, smoothing queue 206 and lookaheadqueue 208 in a pipeline manner. The operations may include toleranceunderstanding on the elements in tolerance queue 202, compression on theelements in compression queue 204, preparatory, corrective, anddouble-arc smoothing on the elements in smoothing queue 206, third-ordersmoothing (if necessary), pre-lookahead (if necessary), and lookahead onthe elements in lookahead queue 208.

The queue manager may call the related routines to manage the queuepipeline by performing the following operations: 1) stuff tolerancequeue 202 with NC commands received from the NC interpreter; 2) processthe NC commands in tolerance queue 202: if it is an E code, then get thetolerance information from this command and store the specifiedtolerance region and the related tolerance value; else, if it is anon-TBC motion command, calculate the required tolerance for thiscommand according to the available tolerance information, and attach therequired tolerance value to this command; 3) take NC commands fromtolerance queue 202 and add them to compression queue 204; 4) merge thevery short line moves to a longer one, i.e., replace the very short lineelements by a longer line element in compression queue 204; 5) take NCcommands from compression queue 204 and add them to smoothing queue 206;6) examine all the line elements in smoothing queue 206 to identifythose elements to be smoothed, and make local adjustments for thoseelements; 7) convert those line elements into arc elements; 8) take NCcommands, mostly arc elements, from smoothing queue 206 and add them tolookahead queue 208; 9) check each newly added command and look back tothe previously processed commands in lookahead queue 208; 10) determinea proper velocity for the new command, and modify the velocities for allthe previous commands if necessary; and 11) take NC commands fromlookahead queue 208 and send them to the motion control kernel forexecution.

All the above operations on the queues may be performed concurrently.The data process in queues may continue whenever queues are not full.The Queue Manager may call a first routine to perform all the queueoperations and move the NC commands through the queue pipeline until allof the queues are full or the NC program is entirely loaded. The firstroutine may call a second routine to perform the tolerance, compressionand smoothing operations, and may call a third routine to perform thelookahead operation.

The motion kernel module may perform the tolerance based motion controland may control the motion associated with program lines and arcs. Themotion kernel may replace the non-TBC point-to-point control with thepredictive and true arc motion, wherein a line or an arc is the basictrajectory unit. The control may push the axes along the programmedtrajectory, correcting for deviations as they are encountered.

A non-TBC CNC controller may interpolate a programmed trajectory intosequential target points with required velocities, and may load theminto a PID motion control card for execution. The non-TBC CNC controllermay support time-based interpolation, i.e., breaking up the line/arcdata into position points and velocity points of fixed (or variable)time intervals. Then, the motion control card may perform PID loops tominimize the following error, i.e., the lag between the instantaneousposition and the interpolated target point. Intuitively, imagine anelastic string with one end attached to a motion axis, the other endstepping from target point to target point. The length of the stringrepresents the following error. The motion control card may pull theaxis to reduce the length of the string. This motion control is a“pulling” mechanism, wherein line/arc data are interpolated intoposition points and velocity points of time intervals before execution.Individual axes may move independently under the motion control.Further, the trajectory interpolator may be an open loop, incapable ofcontrolling the following errors. Lastly, the target points may bepre-computed, with little adjustment being made for real-timeconditions.

The predictive TBC motion control may be based on a different mechanismthat recognizes line/arc data (no pre-computed interpolation points) anddirectly pushes the axes along the line/arc trajectory. At every controlcycle (which may have a period of 200 μsec, for example), the controlmay read the position feedback, may compare the current position withthe commanded trajectory (a line or an arc), and may determine a pointon the trajectory that is the point closest to the actual currentposition. This point may be defined as the ideal current position. Thedifference between the actual current position and the ideal currentposition may be defined as the instantaneous random position error ortolerance error. The instantaneous random position error, also referredto simply as the “position error”, may be thought of as the deviation ofthe actual current position from the line/arc trajectory.

The TBC tolerance error is different from the non-TBC following error.More particularly, the following error is the difference between theactual current position and the pre-computed target point on one axis.The TBC tolerance error is the vector difference between the actualcurrent position and the ideal current position which may be dynamicallycalculated in real time. In a non-TBC controller, the pre-computedtarget points may be interpolated based on a time interval that isusually on the order of a millisecond. In a TBC control system, theideal current position may be calculated every control cycle. In anon-TBC control system, the motion control card, rather than the hostcomputer, may deal with the following error. In a TBC system, thetolerance error may be directly corrected by TBC. In a non-TBC controlsystem, the following errors of different axes may be handledindependently. In a TBC system, the tolerance errors for different axesmay be handled coordinately by TBC.

The TBC predictive motion control, at every control cycle, may performthe following steps:

-   Calculate the three-dimensional tangential velocity at the ideal    current position of the trajectory according to S-Curve-   Calculate the instantaneous tangential and centripetal acceleration    forces and their predictive compensate vectors-   Calculate the instantaneous random position error-   Determine a three-dimensional random corrective velocity-   Combine the tangential velocity, the predictive compensate velocity    and the random corrective vector together and issue the resultant    velocity to push the axes along the trajectory

The Run System Task may issue servo commands to the machine tool inorder to follow the tool path and associated velocities determined inthe preceding Queue Manager Task. A command designating a portion of thetool path that the machine tool is to follow may be issued once permillisecond, for example. The portion of the tool path may include a oneor more line segments and/or one or more arcs. In an example shown inFIG. 12, a command specifies a line segment extending from (1,4,0) to(5,1,6) in x-y-z space. A servo intended to redirect the tool path tocompensate for any detected errors, i.e., deviations in the actual toolpath from the corrected tool path, may be issued more frequently than acommand. In the embodiment illustrated in FIG. 12, a servo is issuedevery 200 microseconds, i.e., corresponding with each of the points A-F.FIG. 13 illustrates the timing and duration of each servo over thecourse of a one millisecond command. That is, FIG. 13 illustrates thecontrol loop cycle time of the servos. Each servo may have a duration ofless than 35 microseconds. In one embodiment, each servo has a durationof approximately between 10 and 15 microseconds. On the rising edge ofthe servo, the current position or some other parameter of the machinetool may be determined. Over the duration of the servo, calculationsusing the determined parameter may be performed. On the falling edge ofthe servo, an output signal to redirect the machine tool relative to thework piece may be issued. A time period of 200 microseconds is usedrepeatedly herein as an example of a duration of a servo. Of course,however, the cycle time of a servo may be less than or greater than 200microseconds within the scope of the present invention.

After the first 200 microseconds of the command, the tool may bepositioned at point B in FIG. 12. An encoder or some other type ofposition sensing device may sense the actual position of the tool atpoint B. Upon sensing the tool position at point B, a new target point Tto which the tool should be redirected by the servo along a tool subpath1202 may be calculated or otherwise identified. The calculation oftarget tool subpath 1202 may be dependent upon a deviation of the actualposition of the tool from the target tool path. An example of such adeviation 1204 is shown in FIG. 12. The tool may continue to move pastthe first actual position B during the calculation of tool subpath 1202.The Run System Task of FIG. 2 includes a servo 220 having a computeposition step 222, an S-Curve step 224, a Predictive Error Compensationstep 226, and a Random Error Compensation step 228. The tool may haveveered off to point B instead of following the linear tool path from (1,4, 0) to (5, 1, 6) due to random errors as well as predictive errorsthat are systematic and repeatable. The predictive errors may beidentified via empirical analysis of historic tool position data. Basedupon the predictive error, it may be determined that redirecting thetool to point T is most likely to result in the tool actually movingnear the desired linear tool path in a smooth motion from point B.

S-Curve step 224 includes calculating desired acceleration levels alongtarget tool subpath 1202. More particularly, the acceleration needed tocomplete the servo from point B to point T, i.e., complete target toolsubpath 1202, within a desired period of time and at a desired finalvelocity may be determined. In actuality, the tool may arrive at point Cdue to random errors and unidentified predictive errors. Afteradditional servos, the tool arrives at points D, E and F. Point F may bereached at the end of the one millisecond command. More particularly,subsequent target tool subpaths may be calculated at the completion ofeach servo in a manner substantially similar to the calculation oftarget tool subpath 1202 as described above. The target tool path from(1, 4, 0) to (5, 1, 6) and the target tool subpath 1202 are shown inFIG. 12 as being linear. However, it is to be understood that any targettool path or any target tool subpath may be arcuate.

The second function performed by the Tolerance Controller is thePredictive Error Compensation (step 106 of FIG. 1 and step 226 of FIG.2), which may be part of a “motion kernel.” The motion kernel replaces anon-TBC point-to-point control technique with predictive and true arctrajectory motion, wherein lines and arcs are the basic trajectory unit.The predictive control pushes the axes along the programmed trajectoryand corrects for deviations as they are encountered.

In one embodiment, at every 200 μsec control cycle, the ToleranceControl may read the position feedback, compare the current positionwith the commanded trajectory (a line or arc), and find a point on thetrajectory that is the closest to the current actual position. Thispoint on the trajectory may be defined and referred to as the currentideal position. The difference between the current actual position andcurrent ideal position as dynamically calculated in real time is definedas the position error, which may be dynamically calculated in real time.

Next, the predictive control may analyze the motion trajectory and themachine condition to compensate for repeatable motion errors. Thecorrection for these errors may be predicted by using a parameterizedmathematical model of the machine that has been configured by measuringactual dynamic machine responses. Predictive motion control may analyzethe motion trajectory and the machine condition to make appropriatecompensations to certain types of motion errors, which are repeatableand can be measured by experiments. The position error may then becombined with the predictive corrections to compute the new velocitycommand for each axis. The predictive correction that may be done ateach control cycle is illustrated in FIG. 14.

The predictive error compensation may include Centripetal ForceCompensation for Arc Motion, Tangential Acceleration Compensation,“Stick” Friction Compensation, Leadscrew and Backlash Compensation,Spindle Softness Compensation, and System Delay Compensation, which areeach described in turn below.

The predictive control may precisely machine an arc move withoutlinearizing the arc into many line segments. Under Centripetal ForceCompensation for Arc Motion, using the laws of physics, the predictivecontrol may instantaneously and in real time calculate the centripetalforce needed to pull or hold the axes on the arc at each ideal currentposition according to the current velocity and the arc trajectorycurvature. The centripetal force may be converted to a predictivecompensate vector, which may be instantaneously added to the velocityissued to the motion axes.

A centripetal force vector, based on the current velocity and the arccurvature, is added to a predictive compensation vector that adjusts thevelocity command issued to the motion axes. This correction may enablethe machine to cut highly accurate arcs. The predictive motion allowsthe direct precision machining of arc segments without the conversion ofarc trajectories into a straight-line approximation.

Issuing a proportional velocity to multiple axes may not guarantee thata three-dimensional move is linear, i.e., in a straight line. The motionaxes involved may not go proportionally as required, because theacceleration/deceleration for different axes depend on their dynamics,which changes from axis to axis. Rather than the required straight-linemove, the move may be curved due to differences in axisacceleration/deceleration caused by differing axis dynamics such as beltcompliance or servo amplifier gain. Under Tangential AccelerationCompensation, the predictive control may analyze the acceleration foreach individual axis and calculate the appropriate compensation forcesfor the axes. These compensation forces may be converted to thepredictive compensation vector to generate the required axisaccelerations. This correction causes the axes to be coordinated and toaccurately track the programmed trajectory. These compensation forcesmay be converted to the predictive compensate vector to generate therequired axis accelerations.

The stick friction or static friction of a machine tool has considerableinfluence on the motion control. An axis starting from rest requires anextra kick of energy to overcome the stick friction and begin moving.Under Stick Friction Compensation, to correct and compensate for thestick friction effect, the predictive motion uses a stick frictioncompensator that may trigger when an axis changes its motion direction,i.e., crosses a zero velocity point. The compensation profile is a notchshape, the height and width of the notch is a function of the velocityand the local curvature of the trajectory. The stick friction isdifferent from machine to machine, and from axis to axis. Thus, thecompensation is a function of the velocity and the local curvature ofthe trajectory, and is different from machine to machine and from axisto axis. This compensation may eliminate problems such as flat spots onan arc at the axis quadrants. The compensator should be built for eachindividual axis.

Leadscrew and Backlash Compensation may correct and compensate for themechanical inaccuracy and backlash of the machine axis ball screws. Thefunction of getting position feedback is responsible for the leadscrewand backlash compensation. The position values used by the motion kernelare all leadscrew and backlash compensated according to the leadscrewmapping tables. Each time the axes position feedback is read, it may beadjusted by the leadscrew and backlash compensation values stored inleadscrew map tables. This compensation may correct for mechanical axispositioning errors and for “lost motion” during an axis directionchange.

For some machines, the mechanical frame of the Z-axis or the spindle maynot be rigid enough, causing unwanted vibration during machining.Spindle Softness Compensation may modify the motion parameters for onlythe Z-axis to reduce this unwanted vibration. The parameters that aremodified may include the Z-axis velocity, acceleration and jerk, whereinjerk is defined as the change in acceleration per unit time. As a resultof Spindle Softness Compensation, the part surface finish may beimproved.

The machine system response to a motion command is typically delayed formechanical and electrical reasons. For example, there may be amechanical delay between axis motor motion and actual axis motionbecause of the mechanical transmission. There also may be an electricaldelay between the input of a velocity command to the servo amplifier andactual motion on the motor. The predictive control may compensate forthis delay under System Delay Compensation by giving an anticipatedadjustment before it is actually needed. This may provide the necessarycorrection to the motion axes at the time it is needed on the programmedtrajectory.

Via the above-described compensation schemes, the Tolerance Control maycompensate for predictive, recurring errors. The predictive control maypush the axes along the programmed trajectory and the compensation fromthe machine model may compensate for and correct repeatable machineerrors. Both of these adjustments may greatly reduce the position errorbetween the current position and the line/arc trajectory.

In addition to compensating for predictive error, the Tolerance Controlmay correct random error, as illustrated in FIG. 15. Any remainingposition error that is not corrected by the predictive control asillustrated in FIG. 14 may be further reduced by the random errorcorrection mechanisms. By utilizing the predictive motion, whichdirectly pushes the axes along the programmed trajectory and thepre-measured/computed compensation, which corrects the repeatablemachining errors, the TBC greatly reduces the instantaneous positionerror (the deviation of the actual current position from the line/arctrajectory). Random error correction is analogous to driving on a roadand making small adjustments to the steering wheel in order tocompensate for variations in the surface of the road and to thereby stayexactly in the center of the road.

The position deviation resulting from the random error occurred inreal-time is further reduced by using random error correctionmechanisms, which may include Proportional Control, Integral Control,Derivative Control, S-Curve Acceleration and Velocity Control, andDynamic Velocity Adjustments. Each of these random error correctionmechanisms is described in turn below.

Under Proportional Control, the position error may be multiplied by aproportional gain parameter to arrive at the random corrective velocityvector. The resulting random corrective velocity vector may besubtracted from the Predictive Correction velocity, i.e., from thecommanded velocity, to thereby reduce the error.

Under Integral Control, the position error may be integrated using amodified Laplace transform algorithm. A correction value, which isproportional to the above-integrated result, may be computed and addedto the Predictive Correction velocity (commanded velocity). The modifiedLaplace transform algorithm may eliminate position error that persistsover a period of time. A direct integral control may cause undesirableoscillation. A lower frequency modified Laplace transformation may beused to integrate the position errors with much less oscillation.

Under Derivative Control, a derivative value of the position error maybe added to the Predictive Correction velocity to dampen the RandomError correction. This dampening may improve the stability of the RandomError correction. An artificial viscous friction may be added toincrease the damping coefficient of the system so that the system issteadier. This is equivalent to a proportional plus derivative control.

Under S-Curve Acceleration and Velocity Control, changes in velocitythat are defined in the part program or are adjusted by the AdditiveLookahead algorithm may be controlled using S-curve acceleration. TheS-curve acceleration may be computed at each 200 μsec control cycle orservo cycle. Thus, very smooth control of the axes motion and,ultimately, the axes position may be provided. S-Curve accelerationprovides a non-linear acceleration method that softens and smoothes thetransition between the at-velocity phase and theacceleration/deceleration phase.

The S-Curve velocity profile typically has seven phases, includingincreasing acceleration; constant acceleration; decreasing acceleration;at speed (constant velocity); increasing deceleration; constantdeceleration; and decreasing deceleration.

Non-TBC CNC control uses a time-based scheduling mechanism. Thus, theS-Curve may be implemented as a function of time. The function output isthe scheduled position and velocity. The TBC uses the true arctechnology. At each control cycle, the ideal position on the trajectorymay be calculated instead of the scheduled position. Thus, only thevelocity along the trajectory needs to be calculated from the S-Curve.The S-Curve in the TBC system may be implemented as a real-time process.

The velocity may be calculated from the instantaneous path distance atevery control cycle. This mechanism may eliminate the velocity erroraccumulation and may support some real-time requests such as feedoverride, pause, resume and single step.

The S-Curve in TBC may have the typical seven phases of increasingacceleration; constant acceleration; decreasing acceleration; at speed(constant velocity); increasing deceleration; constant deceleration; anddecreasing deceleration. The increasing acceleration phase may include aconstant initial jerk J0. Acceleration may be linearly increased to themaximum acceleration with the initial jerk J0. Velocity may increase asa quadratic function of time.

In the constant acceleration phase, there may be no jerk, i.e., jerk mayequal 0. Acceleration may remain at the maximum acceleration. Velocitymay increase as a linear function of time.

In the decreasing acceleration phase, jerk may equal −J, wherein J isthe maximum jerk. Acceleration may linearly decrease from the maximumacceleration to 0 with the negative maximum jerk −J. Velocity mayincrease as a quadratic function of time.

In the at speed phase, jerk and acceleration both equal 0. Velocity isequal to the final constant velocity.

In the increasing deceleration phase, jerk may equal −J, wherein J isthe maximum jerk. Deceleration may linearly increase from 0 to themaximum deceleration with the maximum jerk J. Velocity may decrease fromthe final velocity as a quadratic function of time.

In the constant deceleration phase, there may be no jerk, i.e., jerk mayequal 0. Deceleration may remain at the maximum deceleration. Velocitymay decrease as a linear function of time.

In the decreasing deceleration phase, jerk may equal the maximum jerk J.Deceleration may decrease linearly from the maximum deceleration to 0with jerk being equal to the maximum jerk J. Velocity may decrease as aquadratic function of time.

During the lookahead processing, the additive lookahead algorithmdiscussed above may attach a stop distance to each motion command. Thestop distance may be used in the S-Curve implementation to guaranteethat there is enough distance in which to accelerate/decelerate properlybetween velocities.

In one embodiment, S-Curve computation involves the use of severalvariables, such as distance_to_go to represent the trajectory lengthfrom the current position to the target position of a command. Variablestop_distance_left represents the sum of distance_to_go and the value ofthe stop_distance attached to the command. The values of distance_to_goand stop_distance_left may vary with the instantaneous position of themachine. Variable stop_distance_required represents the moving distancerequired to completely stop the machine according to the velocity,deceleration and the S-Curve for the command. The value ofstop_distance_required may vary with the velocity.

When a motion command is being executed, at every control cycle, thevariables stop_distance_left and stop_distance_required for this commandmay be calculated and compared. Whenever the value of stop_distance_leftis less than or equal to the value of stop_distance_required (not enoughdistance is left), a velocity/distance conversion algorithm may be usedto calculate the velocity from the stop_distance_left, as described inmore detail below. When the value of stop_distance_left is greater thanthe value of stop_distance_required (enough distance is left), thevelocity in each of the seven phases may be calculated as describedbelow.

In Phase 1, acceleration is increased linearly according to the initialjerk. Acceleration may be calculated according to the equation:acceleration_(i+1)=acceleration_(i)+J0×cycle_timewherein J0 is the initial jerk and cycle_time is the time interval ofeach control cycle, which may be approximately 200 μsec. Velocity isincreased according to the acceleration value. Velocity may becalculated according to the equation:velocity_(i+1)=velocity_(i)+acceleration×cycle time.When acceleration reaches the maximum_acceleration, it may be truncatedas follows:If (acceleration>=maximum_acceleration), then set:acceleration=maximum_acceleration;and enter Phase 2.

In Phase 2, acceleration equals maximum_acceleration. Velocity may becalculated according to the equation:velocity_(i+1)=velocity_(i)+maximum_acceleration×cycle time.The difference between the final_velocity and current velocity isexpressed by the equation:final_velocity−velocity=0.5×J×t²,wherein J is the maximum jerk, and t is the time required to reach thefinal velocity. Since the acceleration equals the product of jerk andtime, i.e., J×t, the equation above may be rearranged as:2×J×(final_velocity−velocity)=acceleration²oracceleration=√{square root over (2J(final_velocity−velocity))}.If acceleration is less than the maximum_acceleration, then Phase 3(decreasing acceleration) may be entered.

In Phase 3, velocity may be calculated by the equations:acceleration=√{square root over (2J(final_velocity−velocity))}.velocity_(i+1)=velocity_(i)+acceleration×cycle_time.If velocity reaches final_velocity, then Phase 4 (steady state speed)may be entered.

In Phase 4, velocity equals the maximum_velocity, and accelerationequals 0.

In each of Phase 5 (increasing deceleration), Phase 6 (constantdeceleration), and Phase 7 (decreasing deceleration), the velocity maybe calculated using a velocity/distance conversion algorithm.

At the control cycle when the value of stop_distance_left is less thanor equal to the value of stop_distance_required (the conditionautomatically satisfies in Phases 5, 6 and 7), the velocity/distanceconversion algorithm may be used to calculate the velocity from thestop_distance_left. The key features of one embodiment of this algorithmare described below.

Since S-Curve is a function that defines the relation between velocityand time, it can be represented by the velocity equation:velocity=S_Curve (t).

Let t₁ be the current time, and let t₂ be the time when the motioncompletely stops. The time required for the motion to completely stop isthen given by t₂−t₁. The algorithm may cause the value ofstop_distance_required to be equal to the value of stop_distance_left byselecting a proper velocity.

The moving distance of the machine during t₂−t₁ may be defined as thestop_distance_required at time t₁. The stop_distance_required at time t₁may be calculated by taking the integral of the velocity equation withrespect to the time t as per the equation:stop_distance_required=∫^(t2) _(t1)S_Curve(t) dt.The above equation defines the relation between stop_distance and time.For simplicity, the equation may be rewritten as:stop_distance_required=Stop_Dis(t).Therefore,t=Stop_Dis⁻¹(stop_distance_required)wherein Stop_Dis⁻¹ is the inverse function of Stop_Dis.Substituting for t in the velocity equation yields:velocity=S_curve(Stop_Dis⁻¹(stop_distance_required)).In order to make the value of stop_distance_required equal to the valueof stop_distance_left, the velocity may be set according to theequation:velocity=S_curve(Stop_Dis⁻¹(stop_distance_left)).

In a machine tool, different axes may have different maximum velocities.The maximum velocity of the Z axis is usually lower than the maximumvelocity of the X axis or of the Y axis. Assume maxv_x, maxv_y andmaxv_z are the maximum velocities for axes X, Y and Z, respectively, anddmv is a vector in three-dimensional space. If the machine tool movesalong the direction of dmv, then the maximum allowable vector velocityalong the direction of dmv is a function of the vector dmv:maximum_allowable_velocity=f(dmv).The function is a mapping from a three-dimensional unit vector to a realnumber. A conservative way to determine function f is to define it as aconstant:f(dmv)=Min{maxv_x, maxv_y, maxv_z}

If the machine tool starts from the origin, along the direction of anarbitrary direction vector dmv, and with a given vector velocity f(dmv),and moves for one unit of time, then the machine tool will arrive at acertain end point. The end points resulting from all possible movingdirections will form a three-dimensional surface, which is referred toas the velocity envelope. The shape of the velocity envelope depends onthe function f. In this case, the distance from an end point to theorigin is a constant Min{maxv_x, maxv_y, maxv_z}, and thus the velocityenvelope is a sphere.

An aggressive strategy is to move the machine tool along direction dmvwith the highest possible vector velocity, provided the velocities ofaxes X, Y, and Z are bounded by maxv_x, maxv_y and maxv_z, respectively.In this case, the velocity envelope is a three-dimensional cubic box,with length 2×maxv_x, width 2×maxv_y, and height 2×maxv_z.

The velocity envelope can be used to define the velocity coordinate typeof axes. There are four velocity coordination types, in order ofdecreasing aggressiveness: AXIS_VELOCITY_CUBIC_TYPE,AXIS_VELOCITY_CYLINDER_TYPE, AXIS_VELOCITY_ELLIPSE_TYPE, andAXIS_VELOCITY_SPHERE_TYPE. The TBC may use AXIS_VELOCITY_ELLIPSE_TYPE.The velocity envelope of AXIS_VELOCITY_ELLIPSE_TYPE is athree-dimensional ellipse, with X axis length equal to 2×maxv_x, Y axislength equal to 2×maxv_y and Z axis length equal to 2×maxv_z.

The TBC supports the dynamic velocity adjustment. Under Dynamic VelocityAdjustments, the Tolerance Control may monitor the position error andcompare the error with the specified tolerance. A safety level, definedas a percentage of the required tolerance, may be used to control theposition error. If the position error is beyond the safety level, theaxes velocity may automatically be decreased in order to reduce theposition error. If the position error is below the safety level, theaxes velocity may automatically be increased to the original velocity.In this way, the machine may be allowed to run as fast as possible, ornearly so, while still maintaining the desired tolerance. The dynamicvelocity adjustment may be based on a Laplace transformation.

The TBC motion kernel of the present invention may take the motioncommands from the lookahead queue and control the servo system to trackthe contour trajectory. The TBC motion control is different from non-TBCmotion control in that it is a closed-loop contour tracking control. Acomparison between non-TBC motion control and TBC motion control isprovided below.

Non-TBC Motion Control Structure

In order to track a three-dimensional contour trajectory, non-TBC motioncontrol system may decompose the three-dimensional contour into X/Y/Zaxes and interpolate the X/Y/Z motion with respect to time. Each axismay have its own time based reference trajectory and may use feedbackand feedforward control to track its own motion trajectoryindependently. Ideally, if each axis tracks its reference trajectoryperfectly, the resulting three-dimensional contour would also beperfectly followed. Due to the existence of disturbances, shown in FIG.37, and modeling errors, perfect tracking is normally not achievable andthere may be tracking errors between the actual axis motions andreference trajectories. There may be an attempt to minimize the trackingerror in each axis without any teamwork or coordination among the threeaxes. The trajectory of each axis may be tracked without knowing orcaring what the state of the trajectory in other axes is. For example,even if the trajectory in one axis meets a large resisting force andlags behind its reference trajectory, there may be an attempt to trackthe preset trajectories of the other two axes. The result is that eventhough the trajectory may be tracked perfectly though two of the threeaxes, the actual contour that is achieved may still deviate from thereference contour significantly.

The non-TBC motion control system illustrated in FIG. 37 may be timedeterministic. That is, the x/y/z axis motion trajectories may be timebased and the entire system's runtime may be accurately determinedbeforehand. F_x, F_y and F_z may indicate feedforward controllers foreach of the three axes; and C_x, C_y and C_z may indicate feedbackcontrollers for each of the three axes. The contour tracking may be openloop in non-TBC motion control. Motion along each of the three axes maybe independent without knowledge of, or dependence upon, motion alongany other axis. In other words, there may be no teamwork among the axes.There may be no arc motion even with circular interpolation in non-TBCmotion control. Different dynamic responses in the axes may negativelyaffect tracking accuracy and surface quality.

In non-TBC motion control the contour tracking may be open loop, andthere may be no guarantee that the actual contour achieved is closeenough to the desired reference contour. An example of uncoordinatedmotion is given in FIGS. 38 a-d, where a reference trajectory y=x (FIG.38 a) is decomposed into time based axis trajectories: y=kt (FIG. 38 b)and x=kt (FIG. 38 c). The non-TBC motion controller may control motionin the x and y axes to track their trajectories independently. Twodifferent cases are represented in FIGS. 38 b-d. Case 1 is characterizedby small tracking errors in both x and y axes; however the contouringerror as shown in FIG. 38 d is relatively large. Case 2 is characterizedby larger tracking errors in both x and y axes, but there is nocontouring error at all shown in FIG. 38 d. Because the non-TBC motioncontrol reduces the tracking error in each axis individually without anycoordination among axes, the non-TBC motion system may consider case 1to be more desirable than case 2.

When a contour is decomposed into x/y/z axes and becomes threetime-based axis trajectories, important kinematics information, e.g.,centripetal acceleration information, may be lost. Without thecentripetal acceleration information, a factor that distinguishes arcmotion from line motions, true arc motion may not be possible. Moreover,because the effects of centripetal force may not be compensated for, theactual tool position may be biased in a radially-outward direction dueto the centripetal force.

In non-TBC motion control, the dynamic response of each axis may affectthe contouring performance. If the dynamics are not matched with eachother, e.g., some axis responds faster than others or some axis respondsslower than others, the actual contour may distort or deviate from thedesired contour.

TBC Motion Control Structure

TBC motion control does not involve decomposing a three-dimensionalcontour into three independent time-based axis trajectories. Rather, TBCmotion control may compare the actual contour position with a referencecontour and determine a control signal to minimize the contour error.One embodiment of TBC motion control structure is shown in FIG. 39,where all signals are vectors and represented by thick lines. Incontrast, in the non-TBC motion control structure shown in FIG. 37,signals are scalars and are represented by thin lines.

The TBC motion control structure of FIG. 39 may employ contour-basedfeedback plus feedforward control. The contour tracking may beclosed-loop, and true arc motion may be possible. That is, the tool maybe commanded to follow an arcuate trajectory rather than a trajectoryformed by a series of linear segments that approximate a curvedtrajectory. TBC motion control may not be time deterministic, and may becapable of on-the-fly feedrate adjustments.

At every control cycle, such as every 200 microseconds, the controllermay read the actual contour position feedback, compare the actualposition with the reference contour, and find out the closest point onthe reference contour to the actual position. This point is defined asthe ideal current position (ICP). The controller may include threefunctional blocks: feedforward, feedback and stick friction/backlashcompensation as illustrated in FIG. 40 and described below.

At every control cycle, the controller may calculate the desired motionbased on the ICP (including acceleration and velocity in both normal andtangential directions), and may include a feedforward controller toconvert the desired motion into a velocity control signal. A runtimefeedrate adjuster may also be added to adjust the feedrate (velocity)for tolerance control purposes.

The controller may also calculate the tracking errors in both normal andtangential directions at every cycle. The error in the normal directionis the distance from the actual contour position to the ICP, i.e., thetolerance error or contour error. The error in the tangential directionis defined as the amount of path length that the ICP lags behind.Because the normal error may be important to the system performance, theTBC controller may include two different feedback controllers for thenormal and tangential errors and may convert the tracking errors into avelocity control signal. The stick friction and backlash compensationmay further improve the accuracy of the machine tool.

As compared with the non-TBC motion control, an advantage of the TBCmotion control is the closed-loop structure. The closed-loop contourtracking control may coordinate the three axes and minimize the contourerror. For example, if the x-axis has a large resisting force and lagsbehind its reference trajectory, the other two axes could slow down tomatch the x-axis motion so that the actual contour follows the referencecontour. The TBC motion kernel may also enable true arc motion becausethe centripetal acceleration may be considered in the control.

Because the desired motion at each control cycle is based on thecontour, the ICP, and the current system condition, such as thetolerance error, the system may no longer be time deterministic.However, on-the-fly feedrate adjustment may be possible. When thecontour/tolerance error is below the specified value, the systemfeedrate may be increased. Conversely, when the contour/tolerance errorexceeds the specified value, the feedrate may be reduced such that theerror is also reduced. In other words, the system may maximize thefeedrate while maintaining the contour/tolerance error within aspecified range for optimal system runtime or throughput.

The motion kernel may receive motion commands from lookahead queue 208.The motion commands may go through tolerance understanding processing,data compressing, data smoothing, and lookahead processing before beingexecuted by the motion kernel. Each command may carry the requiredtolerance value for TBC control and the stop distance for S-Curvecomputation. The motion kernel may be executed every 200 μsec in RTSSenvironment wherein a deterministic response is ensured.

FIG. 16 illustrates one embodiment of a method 1600 of the presentinvention for executing a motion kernel. In a first step 1602, anelement is taken from lookahead queue 208. The element may be the motioncommand to be executed next. Next, in step 1604, a routine may be usedto initialize and calculate some variables and parameters which areneeded for executing the command just taken in step 1602. In step 1606,a routine that is the main part of the motion kernel is executed. Theroutine of step 1606 may perform the predictive motion, true arc move,tolerance control and S-Curve computation. In step 1608, a routine maydetermine whether the execution of the current motion command has beencompleted. If so, operation is returned to step 1602 to take anothermotion command from lookahead queue 208. If not, operation is returnedto step 1606 to continue performance of the predictive motion, true arcmove, tolerance control and S-Curve computation.

Step 1606 may include twenty substeps as described below, and referredto in FIGS. 39 and 40, in order to convert a motion command into a servoamplifier digital-to-analog converter (DAC) command, or a servoamplifier command for use with pulse width modulation and digitalcontrols and drives. These twenty substeps may be performed within servo220. In Step 1, the current tool position is obtained, and the positionmay be leadscrew and backlash compensated.

Position geometry analysis is performed in Step 2. More particularly,four computations may be made: the ideal current position on the arc orline trajectory; the current tangential moving direction; the positionaldeviation from the ideal position; and the tolerance error.

Step 3 includes performing lag distance delay compensation. The lagdistance required to compensate time delay in the system may becomputed, and this lag distance may be added to the stop distance whichis used to calculate the velocity by the S-Curve.

S-Curve velocity control is performed in Step 4. The tool velocity maybe calculated as a function of stop distance according to the S Curve.The output is relative speed, having a value between 0 and 1.

Modified Laplace transformation and tangential predictive control isperformed in Step 5. The current velocity may be estimated by themodified Laplace transformation. Tangential predictive control may beadded.

Step 6 includes velocity control for axes. Velocity components may becomputed for each axis. The computation may be based on the velocitycalculated at step 4 and the tangential moving direction computed atstep 2.

Acceleration is estimated in Step 7. The centripetal force as well asthe compensation vector for the centripetal compensation may beestimated. The order, i.e., sequence, of Steps 5 through 7 is arbitraryand may be changed in the program code.

Step 8 includes centripetal predictive compensation in which thecentripetal compensation vector may be added to velocity. Step 9includes statistical analysis, and may be used for research.

In Step 10, the current moving direction is computed. The current movingdirection may be needed for looking up the leadscrew mapping tables toimplement the leadscrew/backlash position compensation. Step 11 includesviscous friction estimation in which the error velocity is estimated.

Step 12 includes random error correction and viscous frictioncorrection. The random error correction vector may be calculated, andthe viscous friction may be added thereto.

A run time velocity adjustment is performed in Step 13. It is determinedwhether the position error is within the safety tolerance level. If so,the velocity slow down function may be disabled. If not, the velocityslow down function may be enabled, which may decrease the velocityaccordingly.

Step 14 includes a variable gain calculation in which the constant gain,the first order gain, the second order gain, and the combined gain maybe calculated. In Step 15, machine vibrations may be analyzed,compensated for, and/or eliminated.

Random error correction may be performed in Step 16. The random errorcorrection vector may be multiplied by the combined gain. The randomerror correction vector may be truncated and/or applied to velocity.

In Step 17, the velocity error may be reduced by using a modifiedLaplace transformation. Step 18 includes motor saturation protection.The DAC voltage change may be detected at every cycle. The DAC voltagevalue may be truncated if it is too high.

Velocity coordination may be performed in Step 19. Machine movement foraxes may be coordinated with non-uniform maximum velocities.

In Step 20, a velocity command, ranging from −10 to +10 volts, may betransmitted in order to control velocity.

The table below illustrates which of the twenty steps are performed inconverting three types of motion commands into a servo amplifier DACcommand. A “Rapid Line” motion command initiates a straight line movethat may be executed at the maximum feedrate for each axis in the move.A Rapid Line command always stops at the end of a move, while a Linecommand may not. A Rapid Line command may be used to move the tool to agiven point as fast as possible. Step Step Name Rapid Line Line Arc 1Get Current Yes Yes Yes Position 2 Position Geometry Distance toDistance to Distance to Analysis go based on go based on go based on endposition. stopper stopper Set err = 0. position. position. Calc err.Calc err. 3 Lag Distance Delay No Yes Yes Compensation 4 S-CurveUncoordinated Coordinated Coordinated Controller Exponential Modified S-Modified S- Deceleration Curve Curve Deceleration Deceleration 5Modified Laplace No Yes Yes velocity trans- 5% cap 5% cap formation 6Component Velocity No Yes Yes Control 7 Acceleration No Yes YesEstimation 8 Centripetal No Yes Yes Predictive Compensation 8.5 IntegralNo Yes Yes Compensation 9 Error Statistical No Yes Yes Analysis 10Backlash No Yes Yes Compensation 11 Viscous Friction No Yes YesEstimation 12 Random Error Yes No No Correction (Method #1) 13 Run TimeVelocity No Yes Yes Adjustment 14 Variable Gain No Yes Yes Calculator 15Vibration No Yes Yes Elimination 16 Random Error No Yes Yes Correction(Method #2) 17 Speed Regulator No No No 18 Motor Saturation Yes Yes YesProtection 19 Velocity Yes Yes Yes Coordination 20 Set Dac Value Yes YesYes

The TBC motion kernel may be deployed on a wide variety of CNC machineplatform types. Like other CNC controllers, parameters may be adjustedto optimize the control's performance for a particular machiningplatform or environment. Adjustment of these parameters is referred toas “tuning”.

The present invention encompasses five categories of tuning parameters,including S-Curve control parameters, smoothing/compression controlparameters, motor speed adjustment parameters, dynamic control gains,and a speed/radius tolerance relation lookup table. The S-Curve controlparameters are adjustable within a reasonable range in order to achievemaximum allowable speed, maximum allowable acceleration, maximumallowable deceleration, maximum allowable start jerk, and/or maximumallowable end jerk. Smoothing/Compression control parameters are alsoadjustable within a reasonable range in order to achieve first ordersmoothing length limit (segments longer than value are not smoothed),first order smoothing angle limit (angles larger than value are notsmoothed), second order smoothing length limit, and/or data compressionOff/On control. Motor speed adjustment parameters may be manuallyadjustable to achieve M—multiplier (i.e., at X percent of maximumvoltage, X percent of maximum speed is observed), and/or B—constantdrifting adjuster for each axis (electronic zero compensation). Dynamiccontrol gains may be preset for a machine class, and may be manuallymodifiable as needed. Dynamic control gains may include time/responsegain parameters. Excessively high values of the time/response gainparameters may cause oscillations, and low values may cause slow/softmotion response. Lastly, the speed/radius tolerance relation lookuptable may be preset for each class of machines, and may be manuallyadjusted as necessary. The above-described tuning parameters may belisted together with other system configuration parameters in aconfiguration file which may be read and interpreted during the systemconfiguration time.

TBC Geometry Analysis

Motion Command Pre-Analysis

Before executing any motion command, the TBC motion kernel may load thecommand and analyze the geometry. The stopper plane normal (SPN)direction and stopper position may be calculated in the pre-analysis andmay be used through the entire executing period until this motioncommand is finished. For a line motion, the SPN may be calculated asindicated by equation (6) and shown in FIG. 41, which illustrates theline motion geometry pre-analysis: $\begin{matrix}{{SPN}_{i} = \frac{P_{i - 1} - P_{i}}{{P_{i - 1} - P_{i}}}} & (6)\end{matrix}$

For an arc motion, the SPN may be calculated as indicated by equation(7) and shown in FIG. 42, which illustrates arc motion geometrypre-analysis, where APN stands for the unit direction vector of the arcplane normal direction, and C stands for the center of the arc.$\begin{matrix}{{SPN}_{i} = {{- {APN}} \times \frac{P_{i - 1} - C_{i}}{{P_{i - 1} - C_{i}}}}} & (7)\end{matrix}$

The stopper position Psi may be calculated as indicated by equation (8)for both line and arc motions.Ps_(i)=P_(i)−ds_(i)·SPN_(i)  (8)Real-time Geometry Analysis

At each control cycle, the TBC motion kernel may read the actual contour(tool) position feedback Ptl and compare the actual position with thereference contour to find the ideal current position (ICP),contour/tolerance error (e_(n)) and the distance_to_go d (to thestopper).

For a line motion, the geometry is shown in FIG. 43, which illustratesline motion geometry analysis, and may be calculated as indicated byequations (9) and (10).d=<Ptl−Psi, SPN>  (9)e_(n)=(Ptl−Psi)−d·SPN  (10)wherein <a,b> is the inner production of vector a and vector b. Thus, ifa=[xa, ya, za], and b=[xb, yb, zb], then <a,b>=xa*xb+ya*yb+za*zb. Ptl isthe tool position, Psi is the stopper position of the i motion segment,and SPN is the unit direction vector of stopper plane normal (SPN)direction. Equation (9) may be interpreted as the distance d being equalto the length of Ptl−Psi in the SPN direction.

For an arc motion, because the actual contour or tool position may notbe on the arc plane, the tool position may be first projected onto thearc plane, as may be calculated as indicated in equation (11) and asshown in FIGS. 44 a-b, which are illustrative of arc motion geometryanalysis.P′tl=(Ptl−Ci)−<Ptl−Ci, APN>·APN  (11)Equation (11) may be interpreted as P′tl being equal to the unit arcplane normal direction vector APN being multiplied by a differencebetween vector (Ptl−Ci) and the vector <Ptl−Ci, APN>.

The vector P′tl is the projection of (Ptl−Ci) on the arc plane. Once thevector P′tl is found on the arc plane, then ICP, the unit tangential andradial direction vectors dt and dr, and contour/tolerance error e_(n)(also referred to as the normal error) may be determined as indicated inequations (12), (13), (14) and (15). $\begin{matrix}{{dr} = \frac{P^{\prime}{tl}}{{P^{\prime}{tl}}}} & (12) \\{{ICP} = {{Ci} + {R \cdot {dr}}}} & (13) \\{{dt} = {{dr} \times {APN}}} & (14) \\{e_{n} = {{Ptl} - {ICP}}} & (15)\end{matrix}$

In order to calculate the distance_to_go, i.e., the path length from ICPto the stopper, the arc length between ICP and target point Pi may firstbe calculated. Any arc that is more than 180 degrees may be cut into twoarcs. The distance_to_go may be calculated by use of the following threeequations (16), (17) and (18). The meanings of the variables ofequations (16), (17) and (18) may be discerned with reference to FIGS.44 a-b. $\begin{matrix}{l = {\frac{1}{2}{{{Pi} - {ICP}}}}} & (16) \\{a = {\sin^{- 1}\left( \frac{l}{R} \right)}} & (17) \\{d = {{2{aR}} + {dsi}}} & (18)\end{matrix}$TBC S-Curve Controller

S-Curve acceleration provides a non-linear acceleration method thatsoftens and smoothes the transition between the at-the-speed phase andthe acceleration/deceleration phase. It is referred to as “S-Curve”because the acceleration/deceleration part of the velocity profile isS-shaped instead of linear.

The S Curve velocity profile usually has seven phases: increasingacceleration; constant acceleration; decreasing acceleration; at speed;increasing deceleration; constant deceleration; and decreasingdeceleration.

Non-TBC motion control systems may use the S-Curve to generate referencetrajectories, i.e., scheduled position, velocity, acceleration and soon. The S-Curve is time-based and implemented outside the control loop(real-time portion). The TBC S-Curve controller may be different fromthe Non-TBC S-Curve controller. At each control cycle, based on thedistance to go, the TBC S-Curve controller may calculate the desiredvelocity and acceleration along the reference contour trajectory. TheTBC S-Curve controller may be distance-based and implemented as a realtime process. The velocity may be calculated from the instantaneous pathdistance at every control cycle. This mechanism may reduce the velocityerror accumulation, and may also support some real time requests such ason-the-fly feedrate adjustment, feed override, pause, resume and singlestep.

A flow chart of one embodiment of a method of operation 4500 of a TBCS-Curve controller is shown in FIG. 45. In a first step 4502, the flagis checked to see if the current system status is “accelerate” or“decelerate.” If the status is decelerate, operation proceeds to step4512. On the other hand, if the current status is “accelerate”, thestop_distance_required is calculated in step 4504. Thestop_distance_required may be defined as the minimum distance that thetool would travel according to the S-curve before the system could bringthe tool to a stop. The stop distance may be a function of current speedand acceleration. In step 4506, if the given distance_to_go “d” isgreater than the stop_distance_required “d0”, which means that it issafe to continue to move forward, then operation proceeds to step 4512to calculate the desired acceleration and velocity. Otherwise, if thedistance_to_go “d” is less than the stop_distance_required “d0”, whichmeans the system must brake immediately, then brake information iscalculated and the flag is set to decelerate in step 4508. Because thedistance_to_go “d” is not great enough for the system to brake accordingto the original S-Curve (with preset maximum jerk and acceleration),maximum jerk may be modified in step 4510 so that the system does notovershoot. Two distances, d1 and d3, which may be used to detect theS-Curve phase, may also be updated in step 4510. Step 4512 may beconsidered the core of the S-Curve controller. According to the currentsystem status and the critical distances calculated beforehand, theS-Curve phase, desired velocity and acceleration may be determined instep 4512. Because the S-Curve controller runs in real time, care may betaken to ensure that the controller does not run out of data. When thetotal moving distance of the motion commands at the queue becomes tooshort, the system may need to slowdown to prevent data starvation. Instep 4516, on-the-fly feedrate adjustment and user override is applied.In step 4518, an advanced nonlinear jerk-acceleration-velocity (JAV)filter is applied to remove the noise in the velocity signal.

TBC Feed Forward Control

Feed Forward Signals and Gains

Feed forward may be based on the ideal current position (ICP). Thecalculation of the three feed forward parts, i.e., normal accelerationFFan, tangential velocity FFvt, and tangential acceleration FFat, isshown in FIG. 46, which illustrates velocity and accelerationfeedforward. In FIG. 46, “d” represents the distance_to_go obtained fromthe geometry analysis; and dt and dr represent unit tangential andradial/normal direction vectors, respectively.

In a lag distance compensation block 4602, the lag distance required tocompensate time delay in the system may be computed and added to thedistance_to_go to arrive at a compensated distance. S-Curve controller4604 may receive the compensated distance and calculate the desiredvelocity v and acceleration a (both v and a are scalars) along thetrajectory.

The acceleration a on line 4606 may be multiplied by the unit tangentialdirection vector dt 4608 to produce the tangential acceleration vectorat 4610. The feed forward gain, Kaff, may convert the tangentialacceleration at into a control signal FFat, which is acceptable by theservo system.

The Modified Laplace block 4612 may estimate the current velocity,compare the current velocity with last velocity command, and add apredictive compensation into the S-curve velocity command v to reducethe tangential tracking error. The compensated velocity commandmultiplies the unit tangential direction vector dt 4608 and becomes thetangential velocity vector vt 4614. Because the servo system takes thevelocity control commands, no further conversion may be needed for thevelocity vector. A feed forward gain 1 at 4616 results in a signal FFvtand may enable all channels to be at the same level.

The centripetal acceleration an 4618 may be calculated by the circularmotion physics, i.e., a_(n)= v²/R. The centripetal acceleration may bedirected toward the arc center and opposite of the radial/normaldirection. The same feed forward gain Kaff as applied to the tangentialacceleration may be applied to convert the normal acceleration into asuitable control signal FFan.

Runtime Feedrate Adjuster (μ)

The runtime or “on-the-fly” feedrate adjuster is a unique feature of TBCmotion control, and is not available in other control systems. Thenon-TBC, time-based control strategy's reference contour trajectory ispredetermined outside the control loop, and no modification can be doneinside the control loop. In contrast, the TBC motion kernel may not haveany predetermined reference trajectories, and desired motion may bedetermined inside the control loop at every control cycle based on thecurrent system condition. Thus, the runtime feedrate adjuster may bepossible.

The difference between the actual current position and the ideal currentposition may be defined as the instantaneous random position error ortolerance error. The instantaneous random position error, also referredto simply as the “position error”, may be thought of as the deviation ofthe actual current position from the desired line/arc trajectory. Theruntime feedrate adjuster may compare the position/tolerance error tothe specified error tolerance. If the position error is bigger than, ortoo close to, the specified error tolerance, the runtime feedrateadjuster may slow down the feedrate to improve tracking accuracy. On theother hand, if the error is smaller than, and not too close to, thespecified tolerance, the runtime feedrate adjuster may increase thefeedrate to improve the system throughput. Generally, the speed of thetool may be increased if the position error is below the specified errortolerance, and the speed of the tool may be decreased if the positionerror is above the specified error tolerance. It is possible, in oneembodiment, to not increase the feedrate even if the contour error isvery small, but to retain the ability to either increase or reduce thefeedrate as appropriate.

The runtime feedrate adjuster may be a constrained optimizationmechanism. That is, under the constraint of the contour/tolerance errorwithin a specified range, the runtime feedrate adjuster may maximize thefeedrate to achieve optimal system throughput.

TBC Feedback Control

Normal Error Feedback Control with Variable Gain (τ)

The normal error e_(n), or contour/tolerance error, may be defined asthe shortest distance from the current tool position to the referencecontour, and TBC motion control may reduce this error. Because this is athree-dimensional error, the coordinated motion of the three axestogether may be needed to correct it. The error may also be five orsix-dimensional when additional axes, in the form of rotational axes,are included in the trajectories and in the calculations. Thus, wherethree dimensions or three axes are referred to in the discussion below,it is to be understood that the discussion is equally applicable tomulti-dimensional systems including more than three dimensions and morethan three axes.

A fundamental difference between the TBC and the non-TBC control is thatthe non-TBC control typically decomposes the three-dimensional referencecontour trajectory into three independent axis motions, and controls thethree axes without any knowledge of the contour/tolerance error. The TBCmotion control, in contrast, may directly address the three-dimensionalcontour/tolerance error. The actual position of the tool may be sensedalong each of three axes. A three-dimensional position error between theactual position of the tool in each of the three axes and a desiredposition of the tool in each of the three axes may then be calculated. Athree-dimensional position error signal dependent upon the calculatedthe three-dimensional position error may be produced. The speed and/ordirection of the tool may be controlled dependent upon thethree-dimensional position error signal. As described herein, andillustrated herein with various examples, three-dimensional positionerrors may be calculated by use of vector mathematics.

As is clear from the above description, TBC control may enable motionalong one axis to affect motion control along the other two axes in realtime, i.e., there may be intercoupling between the motion control of thedifferent axes. Non-TBC motion control may not include compensating forinter-axes effects, such as centripetal force, as TBC motion controlmay. Because of the lack of compensation for centripetal force, a toolcontrolled by a non-TBC method may systematically travel radiallyoutward of a desired arcuate trajectory.

As shown in FIG. 47, which illustrates the variable-gain feedbackcontrol of normal error, the TBC motion control may use a variable-gainfeedback control to address the normal error. The error may becalculated in the real time geometry analysis 4002 (FIG. 40). A specialfeedback controller Cn may provide system stability and reduce thenormal error.

The variable gain τ 4702 may be a function of both tangential velocityand normal acceleration. Loosely speaking, the faster the system moves,the larger the gain that is needed to correct the normal error. Thelarger the gain, the more quickly the normal error may be corrected.

Tangential Error Feedback Control

At each control cycle, the TBC may generate a tangential velocitycommand. The integration of the tangential velocity commands may producea position on the reference contour, which may be considered a currenttarget point on the reference contour. The tangential error may bedefined as the path length from the ideal current position (ICP) to thecurrent target point. A feedback controller Ct 4004 (FIG. 40) may reducethe tangential error.

Different Dynamic Responses of the Two Feedback Controls

The TBC feedback control may include two parts: tangential errorfeedback control and normal error feedback control. Instead of treatingthe two errors the same or equally, two feedback controllers may beprovided for the errors, with each feedback controller addressing arespective one of the two errors. A first reason for the differenttreatment of the two errors is that the normal error may be moreimportant than the tangential error in terms of tracking accuracy andsurface finish quality, and thus more weight may be placed on the normalerror. A second reason for the different treatment of the two errors isthat the servo system may have limited power, and it may be desirable touse the power to do more important things first. Thus, two differentfeedback controllers may be used for treating normal and tangentialerrors. The normal error loop may have higher bandwidth or fasterdynamic response. Loosely speaking, more servo power may be used tocorrect normal error than tangential error, and the normal error may bereduced faster than the tangential error.

TBC Stick Friction and Backlash Compensation

The stick friction may have a considerable influence on the motioncontrol. To correct the stick friction effect, the TBC motion kernel mayuse a stick friction compensator. This compensator may be triggered whenan axis changes its motion direction, i.e., across a zero velocitypoint. The compensation profile may have a notch shape, with the heightand width of the notch being a function of the velocity and the localcurvature of the trajectory.

Leadscrews may be used in machine tools as a mechanical transmission toconvert rotary motion into linear motion. The inaccuracy of the ballscrew and backlash may need to be compensated. The position values usedby the motion kernel may be all leadscrew and backlash compensatedaccording to the leadscrew mapping tables.

The compensation block may determine the current moving direction, whichmay be needed for looking up the leadscrew mapping table to implementleadscrew/backlash position compensation. The compensation block mayalso compensate for the stick friction.

TBC Control Signal Protection

The TBC motion control signal may be the sum of the feed forward controlsignal, the feedback control signal and the stick friction compensationsignal. Before sending the control signal to the servo system throughdigital-to-analog converters (DACs), protection procedure may beperformed to avoid overloading the machine. The protection procedure mayinclude two steps, including a first step of limiting the control signalchanging rate. The servo system may receive the velocity command, andthe control signal change rate may correspond toacceleration/deceleration. Although the S-Curve controller may take theacceleration/deceleration into account, it is still possible that theoverall control signal may change too fast due to other reasons, such astracking error feedback, disturbances, stick friction compensation, andso on. In order to avoid exceeding the servo system capacity and forsmooth motion, the control signal change rate may be limited. That is,if the difference between the current and previous control commandsexceeds a threshold, the current command may be limited so that thedifference is saturated at the threshold.

A second step of the protection procedure may be to limit the controlsignal. Both the DAC and the servo system may be capable of receivingonly limited control signal input up to a maximum amount. If the currentcontrol command exceeds this threshold, then steps may be taken tosaturate the current control command to the maximum control input.

FIG. 17 is a block diagram of one embodiment of a machine 1700 of thepresent invention for machining a work piece 1702. Machine 1700 includesa controller in the form of a controller card 1704, an actuator in theform of servo drives 1706 x, 1706 y, 1706 z, 1706 s, a cutting tool1708, and position sensors 1710 x, 1710 y, 1710 z and 1710 s, which maybe in the form of encoders.

The machine tool retains and carries cutting tool 1708 while the cuttingtool rotates and engages the work piece, as indicated by the dashed linebetween the work piece and the cutting tool in FIG. 17. The machine toolprovides a mounting approach and linear/rotational drive approach forthe cutting tool. Cutting tool 1708 is the material remover in machine1700, and may be mounted in the rotating spindle mechanism (not shown)which may be attached to the z-axis of the machine tool. The geometricorientation of the cutting tool with respect to the work piece may befixed due to its mounting position on the z-axis.

As is typical of most motion control cards, controller card 1704includes connectors 1712, 1714, 1716 and 1717 for physically connectingcontroller card 1704 to servo drives 1706 and position sensors 1710 inorder to facilitate communications. Servo drives 1706 may move workpiece 1702 in the x, y and z axes and rotate cutting tool 1708 about thez-axis. Four connectors 1712, 1714, 1716, 1717 are depicted oncontroller card 1704, one for each controlled axis of motion and one forthe rotation of cutting tool 1708, but other configurations are alsopossible. The communication signals exchanged between controller card1704 and servo drives 1706 must often be converted into the appropriateformat. For example, some actuator motors use an analog voltage inputsignal between −10 volts and +10 volts to determine at what rate themotors should turn.

Most digital computers, like the Intel Pentium Based PC, do not supportanalog voltages, but utilize groups of bits (ones and zeros) representedby discrete voltage levels (+5 volts and 0 volts) to represent values.Thus, if a PC based motion control card were to command a motor to spinin the clockwise direction at its maximum speed (+10 volts in ourexample) the collection of bits representing this request would have tobe converted to the +10 volt signal understood by the motor. Thisconversion of signals for communication purposes is standard in theindustry and often performed by the input/output, I/O, module of thecontroller card. In FIG. 17, one I/O module (1718, 1720 and 1722) isdepicted for each controlled axis.

Some control cards incorporate a CPU 1724 and on-board memory 1726 forthe execution of programs and control algorithms. The PMAC offered byDelta Tau and the DMC-1700 by Galil Motion Control, Inc. of MountainView, Calif. is typical of cards incorporating a CPU and memory. Theservo transducer 5-axis PCB Assembly (part no. 415-0622-001), offered byHurco Companies, Inc. of Indianapolis, Ind., may be used as the controlcard in the exemplary embodiment. This Hurco servo transducer does nothave an onboard CPU and memory but utilizes the CPU and memory of thecomputer into which they are plugged for these services. The differencebetween these two configurations is primarily one of cost and is oflittle consequence to a programmer implementing the tolerance basedcontrol technology of this invention. The integration of thesecomponents and the method by which control algorithms are stored andexecuted by the CPU is well understood in the art. The details of thisprocess have been omitted for the sake of brevity. To simplify theexplanation of the invention, FIG. 17 shows CPU 1724 and memory 1726physically integrated on motion control card 1704.

In the exemplary embodiment, a multi-tasking operating system (OS), likeWindows XP offered by the Microsoft Corporation of Redmond Wash., may beemployed for running the algorithms implementing this invention. Areal-time OS, like RTX offered by Ready Systems of Sunnyvale, Calif.could also be used improving the run time performance of the algorithmsas well. The decision of what operating system to use and whether or notto employ multi-tasking paradigm depends on the application at hand andis a decision routinely made by those skilled in the art of computersystem programming and design. Contained in memory 1726 is a data areafor storage of values relevant to the task at hand (1728, motion commandand permitted feedrates; 1730, NC tolerance commands; 1732, feedratelimitation mappings; 1734, other data), as is commonly employed bypeople skilled in the art of programming.

Position sensors 1710 x, 1710 y, 1710 z sense the positions of workpiece 1702 in the x, y and z directions, respectively, as indicated bythe dashed lines between the position sensors and the work piece in FIG.17. More particularly, in one embodiment, position sensors 1710 x, 1710y, 1710 z sense the positions of three respective ballscrew mechanisms(not shown) that drive movable positioning slides (not shown) of themachine tool. The work piece is attached to one of the positioningslides, which, by the nature of the cascaded mounting approach of all ofthe positioning slides, permits the work piece to be moved in twodirections, i.e., the x and y directions. Thus, the position of the workpiece in the x-y plane may be deduced from knowledge of where the workpiece is attached to the x and y positioning slides and knowledge of thepositions of the ballscrew mechanisms. There is generally a fixedgeometric relationship between the work piece, the ballscrew mechanismsand the positioning slides. Given that the rotating spindle mechanism isattached to the z-axis positioning slide, position sensor 1710 z maysense the position of the z-axis (third direction) ballscrew mechanism,which enables the linear position of the cutting tool along the z-axisto be deduced. Position sensor 1710 s senses the rotational position ofcutting tool 1708, as indicated by the dashed line between positionsensor 1710 s and the cutting tool in FIG. 17. Position sensors 1710 x,1710 y, 1710 z, 1710 s may transmit associated position signals to CPU1724 via connectors 1712, 1714, 1716, 1717. Thus, position sensors 1710may provide the Position Feedback depicted in FIG. 1.

FIG. 25 illustrates a method 2500 of the present invention as describedabove with reference to FIG. 12. In a first step 2502, a target toolpath is identified. In the example shown in FIG. 12, a target tool pathfrom (1,4,0) to (5,1,6) is identified. In step 2504, the tool iscommanded to follow the target tool path from an initial position. Forexample, the tool may be commanded to follow the target tool path frominitial position (1,4,0) to (5,1,6). Next, in step 2506, an actualposition of the tool is sensed after the commanding step. In the exampleof FIG. 12, an actual position of the tool at point B is sensed. In afinal step 2508, a target tool subpath from the actual position iscalculated to correct for predictive errors in movement of the tool. Forexample, a target tool subpath 1202 from the actual position at point Bis calculated to correct for predictive errors in movement of the tool.The predictive errors that are corrected may occur anywhere along thepath between points A and F, for example. Further, the predictive errorsmay be empirically determined long before the target tool path isidentified.

FIG. 26 illustrates another method 2600 of the present invention asdescribed above with reference to FIG. 12. In a first step 2602,predictive errors in movement of the tool are empirically determined. Inthe example shown in FIG. 12, predictive errors in the movement of thetool may have been empirically determined through testing before themachine tool has arrived at (1,4,0) or before the machine tool has evengotten within proximity of the work piece. The empirical determinationmay occur in a laboratory, in a manufacturing plant, or in a combinationof both types of environments, for example. The determination of thepredictive errors may be dependent upon ambient temperature and/orhumidity, for example. In step 2604, actual movements of the tool aremonitored. For example, position sensors 1710 may sense the positions ofmachine tool 1708 and transmit associated position signals to CPU 1724as described above. In a final step 2606, the actual movement of thetool are controlled dependent upon the empirically determined predictiveerrors and random errors in the actual movements of the tool. Forexample, the tool may be directed along target tool subpaths, such assubpath 1202, by servo amplifier 1706.

FIG. 27 illustrates yet another method 2700 of the present invention asdescribed above with reference to FIG. 12. In a first step 2702, anactual position of the tool after a first of a plurality of sequentialmovements is sensed. For example, position sensors 1710 may sense theposition of machine tool 1708 after a movement of tool 1708. Next, instep 2704, a target position of the tool is calculated based on thesensing step. The target position is calculated to compensate forpredictive errors and random errors in the movements of the tool. Forexample, CPU 1724 may calculate target position T based upon positionsensors 1710 sensing the position of machine tool 1708 at point B.Target position T may be calculated to compensate for predictive errorsand random errors in the movements of tool 1708. Target position T maybe calculated to maximize the probability of point C, i.e., the actualposition after the servo, being on the target tool path between points(1,4,0) and (5,1,6) after the effects of the predictive errors duringthe servo. In a final step 2706, the sensing and calculating steps arerepeated for other movements of the tool. For example, the position ofthe tool at each of points C, D, E and F may be sensed and a new targetposition may be calculated after and based upon each of these sensingsteps.

One embodiment of a method of trajectory motion control of the presentinvention is shown in pages 1-16 of the TBC Flowcharts and pages 1-5 ofthe TBC Kernel Flowcharts of the attached Appendix I.

While this invention has been described as having an exemplary design,the present invention may be further modified within the spirit andscope of this disclosure. This application is therefore intended tocover any variations, uses, or adaptations of the invention using itsgeneral principles.

1. A method of adjusting path data for a motive body, said methodcomprising the steps of: identifying four consecutive points of the pathdata including a first point, a second point, a third point and a fourthpoint; determining whether the second point is suitable for smoothing;if the second point is suitable for smoothing, then: locating a firstarc defined by the first point, the second point and the fourth point;locating a second arc defined by the first point, the third point andthe fourth point; moving the second point into an area between the firstarc and the second arc; and replacing a line move associated with thesecond point with at least one arc move associated with the secondpoint.
 2. The method of claim 1 wherein the at least one arc move istangent to both a preceding move and a succeeding move.
 3. The method ofclaim 1 wherein the at least one arc move comprises two arc moves, thetwo arc moves being tangent to one another.
 4. The method of claim 1comprising the further step of moving the third point into the areabetween the first arc and the second arc.
 5. The method of claim 1wherein the line move and the at least one arc move are also associatedwith the third point.
 6. The method of claim 1 comprising the furthersteps of: identifying a fifth consecutive point succeeding the fourthpoint; determining whether the third point is suitable for smoothing; ifthe third point is suitable for smoothing, then: locating a third arcdefined by the second point, the third point and the fifth point;locating a fourth arc defined by the second point, the fourth point andthe fifth point; moving the third point into an area between the thirdarc and the fourth arc; and replacing a line move associated with thethird point with at least one arc move associated with the third point.7. A method of adjusting path data for a motive body, said methodcomprising the steps of: identifying three consecutive points of thepath data; ascertaining at least one of: a first distance between acircular arc defined by the three points and a middle portion of a firstline extending between a first of the points and a second of the points;and a second distance between the circular arc and a middle portion of asecond line extending between the second point and a third of thepoints; determining a turn angle between the first line and the secondline; and performing smoothing on the second point if: at least one ofthe first distance and the second distance is less than a thresholddistance; and the turn angle is greater than a threshold angle.
 8. Themethod of claim 7 wherein smoothing is performed on the second point ifa first move from the first point to the second point comprises a firstline move and a second move from the second point to the third pointcomprises a second line move.
 9. The method of claim 8 wherein the stepof performing smoothing includes replacing at least one of the firstline move and the second line move with at least one arc move.
 10. Themethod of claim 7 wherein the threshold line distance is approximatelybetween 0.0002 inch and 0.001 inch.
 11. The method of claim 7 whereinthe threshold angle is approximately between 130 degrees and 160degrees.
 12. The method of claim 7 wherein the first distance is betweenthe circular arc and a midpoint of the first line, and the seconddistance is between the circular arc and a midpoint of the second line.13. The method of claim 12 wherein smoothing is performed on the secondpoint if each of the first distance and the second distance is less thanthe threshold distance.
 14. A method of adjusting path data for a motivebody, said method comprising the steps of: identifying four consecutivepoints of the path data including a first point, a second point, a thirdpoint and a fourth point; locating a first arc defined by the firstpoint, the second point and the fourth point; locating a second arcdefined by the first point, the third point and the fourth point; andmoving the second point into an area between the first arc and thesecond arc.
 15. The method of claim 14 wherein the second point is movedin a direction toward a closest point on the second arc.
 16. The methodof claim 14 comprising the further step of moving the third point intothe area between the first arc and the second arc.
 17. The method ofclaim 16 comprising the further step of determining whether the firstpoint, the second point and the third point are suitable for smoothing.18. The method of claim 17 wherein: if each of the first point, thesecond point and the third point is suitable for smoothing, moving thesecond point less than 45% of a distance between the second point andthe second arc; if the second point is suitable for smoothing, one ofthe first point and the third point is suitable for smoothing, and another of the first point and the third point is not suitable forsmoothing, moving the second point approximately between 45% and 55% ofa distance between the second point and the second arc; and if thesecond point is suitable for smoothing, and neither of the first pointand the third point is suitable for smoothing, leaving the second pointunmoved.
 19. The method of claim 18 comprising the further steps of:identifying a fifth consecutive point succeeding the fourth point;locating a third arc defined by the second point, the third point andthe fifth point; locating a fourth arc defined by the second point, thefourth point and the fifth point; and moving the third point and thefourth point into an area between the third arc and the fourth arc. 20.The method of claim 16 wherein the second point is moved in a directiontoward a closest point on the second arc, and the third point is movedin a direction toward a closest point on the third arc.
 21. A method ofadjusting path data for a motive body, said method comprising the stepsof: identifying a line move extending from a first point of the pathdata to a second point of the path data; and replacing the line movewith a first arc move and a second arc move, the first arc movebeginning at the first point and terminating at a junction point, thesecond arc move beginning at the junction point and terminating at thesecond point.
 22. The method of claim 21 wherein the first arc move istangent to a preceding move at the first point.
 23. The method of claim21 wherein the first arc move is tangent to the second arc move at thejunction point.
 24. The method of claim 21 wherein the second arc moveis tangent to a succeeding move at the second point.
 25. The method ofclaim 21 wherein the first arc move has a first radius and the secondarc move has a second radius different from the first radius.